✨ 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:
@@ -28,6 +28,7 @@
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
|
||||
@@ -106,6 +106,113 @@ public sealed class RuntimeOrchestrationTests
|
||||
Assert.Single(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通轨迹使用准确字串 icsp_rust 时会真实调用 Rust DLL，并在结果中标记原生后端。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_UsesRustPlanner()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
|
||||
var bundle = orchestrator.PlanOrdinaryTrajectory(
|
||||
robot,
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
],
|
||||
new TrajectoryExecutionOptions(method: "icsp_rust"));
|
||||
|
||||
Assert.Equal(PlanningMethod.IcspRust, bundle.PlannedTrajectory.Method);
|
||||
Assert.Equal(PlanningMethod.IcspRust, bundle.Result.Method);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍轨迹使用准确字串 icsp_rust 时会真实调用 Rust DLL，同时保留触发时间轴。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRustPlanner()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
uploaded,
|
||||
new FlyshotExecutionOptions(method: "icsp_rust", useCache: false));
|
||||
|
||||
Assert.Equal(PlanningMethod.IcspRust, bundle.PlannedTrajectory.Method);
|
||||
Assert.Equal(PlanningMethod.IcspRust, bundle.Result.Method);
|
||||
Assert.Single(bundle.Result.ShotEvents);
|
||||
Assert.Single(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通轨迹默认 icsp 仍使用现有 C# 规划器。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_KeepsManagedIcspPlanner()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
|
||||
var bundle = orchestrator.PlanOrdinaryTrajectory(
|
||||
robot,
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
],
|
||||
new TrajectoryExecutionOptions(method: "icsp"));
|
||||
|
||||
Assert.Equal(PlanningMethod.Icsp, bundle.PlannedTrajectory.Method);
|
||||
Assert.Equal(PlanningMethod.Icsp, bundle.Result.Method);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 self-adapt-icsp 仍使用 C# 自适应规划器，不切换到 Rust。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_KeepsManagedSelfAdaptPlanner()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = new RobotProfile(
|
||||
name: "ManagedSelfAdaptTestRobot",
|
||||
modelPath: "Models/ManagedSelfAdaptTest.robot",
|
||||
degreesOfFreedom: 6,
|
||||
jointLimits: Enumerable.Range(1, 6)
|
||||
.Select(static index => new JointLimit($"J{index}", 1.0, 2.0, 10.0))
|
||||
.ToArray(),
|
||||
jointCouplings: [],
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
var uploaded = new ControllerClientCompatUploadedTrajectory(
|
||||
name: "managed-self-adapt",
|
||||
waypoints:
|
||||
[
|
||||
[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]
|
||||
],
|
||||
shotFlags: [false, true, false, false],
|
||||
offsetValues: [0, 0, 0, 0],
|
||||
addressGroups: [[], [7], [], []]);
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
uploaded,
|
||||
new FlyshotExecutionOptions(method: "self-adapt-icsp", useCache: false));
|
||||
|
||||
Assert.Equal(PlanningMethod.SelfAdaptIcsp, bundle.PlannedTrajectory.Method);
|
||||
Assert.Equal(PlanningMethod.SelfAdaptIcsp, bundle.Result.Method);
|
||||
Assert.Single(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
|
||||
/// </summary>
|
||||
|
||||
135
tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs
Normal file
135
tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs
Normal file
@@ -0,0 +1,135 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Native.Planning;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Rust ICSP 托管互操作边界，包括原生库加载失败时的诊断信息。
|
||||
/// </summary>
|
||||
public sealed class RustIcspPlannerInteropTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证显式指定不存在的原生库时，规划器会抛出包含库路径的明确异常。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing()
|
||||
{
|
||||
var planner = new RustIcspPlanner(
|
||||
options: NativeIcspPlannerOptions.Default,
|
||||
libraryResolver: new NativePlanningLibraryResolver(_ => "missing-library"));
|
||||
|
||||
var exception = Assert.Throws<NativePlanningException>(() => planner.Plan(CreateSimpleRequest()));
|
||||
|
||||
Assert.Contains("missing-library", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证实际原生规划结果会被复制为现有 <see cref="Flyshot.Core.Planning.PlannedTrajectory"/>。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult()
|
||||
{
|
||||
var planner = new RustIcspPlanner(
|
||||
NativeIcspPlannerOptions.Default with { EnforceFinalScale = false });
|
||||
|
||||
var result = planner.Plan(CreateSimpleRequest());
|
||||
|
||||
Assert.Equal(PlanningMethod.IcspRust, result.Method);
|
||||
Assert.Equal(result.PlannedWaypointCount, result.WaypointTimes.Count);
|
||||
Assert.Equal(result.PlannedWaypointCount - 1, result.SegmentDurations.Count);
|
||||
Assert.Equal(result.SegmentDurations.Count, result.SegmentScales.Count);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证互操作测试夹具在两套实现中都由相同的最终 scale 规则拒绝。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ManagedIcspPlanner_RejectsInteropFixtureWithExpectedFinalScale()
|
||||
{
|
||||
var planner = new ICspPlanner();
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => planner.Plan(CreateSimpleRequest()));
|
||||
|
||||
Assert.Contains("global_scale=1.058684", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证进程已加载默认 DLL 后，显式指定其他缺失库仍会暴露部署错误。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RustIcspPlanner_DoesNotReuseLoadedLibrary_ForDifferentExplicitPath()
|
||||
{
|
||||
var defaultPlanner = new RustIcspPlanner(
|
||||
NativeIcspPlannerOptions.Default with { EnforceFinalScale = false });
|
||||
_ = defaultPlanner.Plan(CreateSimpleRequest());
|
||||
var missingPlanner = new RustIcspPlanner(
|
||||
options: NativeIcspPlannerOptions.Default,
|
||||
libraryResolver: new NativePlanningLibraryResolver(_ => "different-missing-library"));
|
||||
|
||||
var exception = Assert.Throws<NativePlanningException>(
|
||||
() => missingPlanner.Plan(CreateSimpleRequest()));
|
||||
|
||||
Assert.Contains("different-missing-library", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Rust 未收敛错误码和线程局部错误文本会转换为可诊断的托管异常。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RustIcspPlanner_ConvertsNativeNotConvergedError()
|
||||
{
|
||||
var planner = new RustIcspPlanner(
|
||||
NativeIcspPlannerOptions.Default with { MaxIterations = 0 });
|
||||
|
||||
var exception = Assert.Throws<NativePlanningException>(
|
||||
() => planner.Plan(CreateSimpleRequest()));
|
||||
|
||||
Assert.Equal(3, exception.ErrorCodeValue);
|
||||
Assert.Contains("global_scale", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建用于互操作边界测试的最小四路点单自由度规划请求。
|
||||
/// </summary>
|
||||
/// <returns>满足 ICSP 最小路点数量要求的轨迹请求。</returns>
|
||||
private static TrajectoryRequest CreateSimpleRequest()
|
||||
{
|
||||
var robot = new RobotProfile(
|
||||
name: "rust-interop-test",
|
||||
modelPath: "test.robot",
|
||||
degreesOfFreedom: 1,
|
||||
jointLimits:
|
||||
[
|
||||
new JointLimit(
|
||||
jointName: "J1",
|
||||
velocityLimit: 2.0,
|
||||
accelerationLimit: 4.0,
|
||||
jerkLimit: 8.0)
|
||||
],
|
||||
jointCouplings: [],
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
var program = new FlyshotProgram(
|
||||
name: "rust-interop-test",
|
||||
waypoints:
|
||||
[
|
||||
new JointWaypoint([0.0]),
|
||||
new JointWaypoint([0.5]),
|
||||
new JointWaypoint([1.0]),
|
||||
new JointWaypoint([1.5])
|
||||
],
|
||||
shotFlags: [false, false, false, false],
|
||||
offsetValues: [0, 0, 0, 0],
|
||||
addressGroups:
|
||||
[
|
||||
new IoAddressGroup([]),
|
||||
new IoAddressGroup([]),
|
||||
new IoAddressGroup([]),
|
||||
new IoAddressGroup([])
|
||||
]);
|
||||
|
||||
return new TrajectoryRequest(robot, program, PlanningMethod.Icsp);
|
||||
}
|
||||
}
|
||||
160
tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs
Normal file
160
tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user