✨ feat: 初始化飞拍替换方案仓库骨架
* 建立 .NET 8 解决方案及分层项目结构 * 添加 Flyshot.Core.Domain 领域模型（机器人、轨迹、运动学） * 添加 Flyshot.Core.Planning 规划层（ICSP、CubicSpline、采样器） * 添加 Flyshot.Core.Triggering 触发时序与 IO 时间轴 * 添加 Flyshot.Core.Config 配置兼容与 .robot 解析 * 添加 Flyshot.Server.Host 最小宿主及 /healthz 端点 * 补充单元测试与集成测试项目 * 添加 CLAUDE.md、AGENTS.md、README.md 项目规范
This commit is contained in:
227
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
Normal file
227
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
Normal file
@@ -0,0 +1,227 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Triggering;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 锁定 Task 4 的最小兼容面，覆盖 ICSP、自适应补点以及飞拍触发时间轴。
|
||||
/// </summary>
|
||||
public sealed class PlanningCompatibilityTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证 ICSP 规划至少会生成严格递增的 waypoint 时间轴。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ICspPlanner_ReturnsMonotonicWaypointTimes()
|
||||
{
|
||||
var request = new TrajectoryRequest(
|
||||
robot: CreateRobotProfile([1, 1, 1, 1, 1, 1], [2, 2, 2, 2, 2, 2], [10, 10, 10, 10, 10, 10]),
|
||||
program: CreateProgram(
|
||||
new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.4, 0.1, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.8, 0.3, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 1.0, 0.2, 0.0, 0.0, 0.0, 0.0 }
|
||||
}),
|
||||
method: PlanningMethod.Icsp);
|
||||
|
||||
var trajectory = new ICspPlanner().Plan(request);
|
||||
|
||||
Assert.Equal(4, trajectory.WaypointTimes.Count);
|
||||
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void SelfAdaptIcspPlanner_InsertsMidpoints_ForEol9Case()
|
||||
{
|
||||
var workspaceRoot = GetWorkspaceRoot();
|
||||
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
|
||||
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
||||
|
||||
var config = new RobotConfigLoader().Load(configPath);
|
||||
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
|
||||
var constrainedProfile = ScaleRobotProfile(baseProfile, velocityScale: 0.9, accelerationScale: 0.9 * 0.9, jerkScale: 0.9 * 0.9 * 0.9);
|
||||
|
||||
var request = new TrajectoryRequest(
|
||||
robot: constrainedProfile,
|
||||
program: config.Programs["EOL9_EAU_90"],
|
||||
method: PlanningMethod.SelfAdaptIcsp);
|
||||
|
||||
var trajectory = new SelfAdaptIcspPlanner().Plan(request, adaptIcspTryNum: config.Robot.AdaptIcspTryNum);
|
||||
|
||||
Assert.True(trajectory.InsertedWaypointCount > 0);
|
||||
Assert.True(trajectory.PlannedWaypointCount > trajectory.OriginalWaypointCount);
|
||||
Assert.True(trajectory.SegmentScales.Max() <= 1.0005);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证补点后仍然能按原始示教点顺序找回时间戳，而不是错误地绑定到新增中点。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void WaypointTimestampResolver_UsesOriginalTeachPointsAfterInsertion()
|
||||
{
|
||||
var originalProgram = CreateProgram(
|
||||
new[]
|
||||
{
|
||||
new[] { 0.0, 0.0 },
|
||||
new[] { 1.0, 0.0 },
|
||||
new[] { 2.0, 0.0 },
|
||||
new[] { 3.0, 0.0 }
|
||||
});
|
||||
|
||||
var trajectory = new PlannedTrajectory(
|
||||
robot: CreateRobotProfile([1, 1], [1, 1], [1, 1]),
|
||||
originalProgram: originalProgram,
|
||||
plannedWaypoints:
|
||||
[
|
||||
new JointWaypoint([0.0, 0.0]),
|
||||
new JointWaypoint([0.5, 0.0]),
|
||||
new JointWaypoint([1.0, 0.0]),
|
||||
new JointWaypoint([2.0, 0.0]),
|
||||
new JointWaypoint([2.5, 0.0]),
|
||||
new JointWaypoint([3.0, 0.0])
|
||||
],
|
||||
waypointTimes: [0.0, 0.25, 0.5, 1.0, 1.5, 2.0],
|
||||
segmentDurations: [0.25, 0.25, 0.5, 0.5, 0.5],
|
||||
segmentScales: [1.0, 1.0, 1.0, 1.0, 1.0],
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
iterations: 3,
|
||||
threshold: 0.0);
|
||||
|
||||
var timestamps = new WaypointTimestampResolver().Resolve(trajectory);
|
||||
|
||||
Assert.Equal([0.0, 0.5, 1.0, 2.0], timestamps);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证触发时间轴会使用原始 waypoint 时间、offset 周期和地址组生成 ShotEvent/TrajectoryDoEvent。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ShotTimelineBuilder_MapsOffsetsToShotEventsAndTriggerTimeline()
|
||||
{
|
||||
var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
|
||||
var program = new FlyshotProgram(
|
||||
name: "demo",
|
||||
waypoints:
|
||||
[
|
||||
new JointWaypoint([0.0, 0.0]),
|
||||
new JointWaypoint([1.0, 0.0]),
|
||||
new JointWaypoint([2.0, 0.0])
|
||||
],
|
||||
shotFlags: [false, true, false],
|
||||
offsetValues: [0, 1, 0],
|
||||
addressGroups:
|
||||
[
|
||||
new IoAddressGroup(Array.Empty<int>()),
|
||||
new IoAddressGroup([2, 4]),
|
||||
new IoAddressGroup(Array.Empty<int>())
|
||||
]);
|
||||
|
||||
var trajectory = new PlannedTrajectory(
|
||||
robot: robot,
|
||||
originalProgram: program,
|
||||
plannedWaypoints: program.Waypoints,
|
||||
waypointTimes: [0.0, 0.5, 1.0],
|
||||
segmentDurations: [0.5, 0.5],
|
||||
segmentScales: [1.0, 1.0],
|
||||
method: PlanningMethod.Icsp,
|
||||
iterations: 1,
|
||||
threshold: 0.0);
|
||||
|
||||
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
|
||||
.Build(trajectory, holdCycles: 2, samplePeriod: TimeSpan.FromMilliseconds(16));
|
||||
|
||||
var shotEvent = Assert.Single(timeline.ShotEvents);
|
||||
Assert.Equal(0.508, shotEvent.TriggerTime, precision: 3);
|
||||
Assert.Equal([2, 4], shotEvent.AddressGroup.Addresses);
|
||||
|
||||
var doEvent = Assert.Single(timeline.TriggerTimeline);
|
||||
Assert.Equal(1, doEvent.WaypointIndex);
|
||||
Assert.Equal(1, doEvent.OffsetCycles);
|
||||
Assert.Equal(2, doEvent.HoldCycles);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
|
||||
/// </summary>
|
||||
private static RobotProfile CreateRobotProfile(
|
||||
IReadOnlyList<double> velocityLimits,
|
||||
IReadOnlyList<double> accelerationLimits,
|
||||
IReadOnlyList<double> jerkLimits)
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: "TestRobot",
|
||||
modelPath: "Models/Test.robot",
|
||||
degreesOfFreedom: velocityLimits.Count,
|
||||
jointLimits: velocityLimits
|
||||
.Select((velocity, index) => new JointLimit(
|
||||
$"J{index + 1}",
|
||||
velocity,
|
||||
accelerationLimits[index],
|
||||
jerkLimits[index]))
|
||||
.ToArray(),
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造一个默认不带拍照点的最小 FlyshotProgram。
|
||||
/// </summary>
|
||||
private static FlyshotProgram CreateProgram(IEnumerable<double[]> waypointRows)
|
||||
{
|
||||
var rows = waypointRows.ToArray();
|
||||
return new FlyshotProgram(
|
||||
name: "demo",
|
||||
waypoints: rows.Select(row => new JointWaypoint(row)).ToArray(),
|
||||
shotFlags: Enumerable.Repeat(false, rows.Length).ToArray(),
|
||||
offsetValues: Enumerable.Repeat(0, rows.Length).ToArray(),
|
||||
addressGroups: Enumerable.Range(0, rows.Length).Select(_ => new IoAddressGroup(Array.Empty<int>())).ToArray());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在不丢失 couple 元数据的前提下，按比例缩放机器人关节限制。
|
||||
/// </summary>
|
||||
private static RobotProfile ScaleRobotProfile(RobotProfile source, double velocityScale, double accelerationScale, double jerkScale)
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: source.Name,
|
||||
modelPath: source.ModelPath,
|
||||
degreesOfFreedom: source.DegreesOfFreedom,
|
||||
jointLimits: source.JointLimits
|
||||
.Select(limit => new JointLimit(
|
||||
limit.JointName,
|
||||
limit.VelocityLimit * velocityScale,
|
||||
limit.AccelerationLimit * accelerationScale,
|
||||
limit.JerkLimit * jerkScale))
|
||||
.ToArray(),
|
||||
jointCouplings: source.JointCouplings,
|
||||
servoPeriod: source.ServoPeriod,
|
||||
triggerPeriod: source.TriggerPeriod);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位父工作区根目录，读取真实的 EOL9 样本和机器人模型。
|
||||
/// </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.");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user