✨ 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:
82
src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs
Normal file
82
src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs
Normal file
@@ -0,0 +1,82 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
|
||||
namespace Flyshot.Core.Triggering;
|
||||
|
||||
/// <summary>
|
||||
/// 根据规划轨迹和飞拍配置生成触发时间轴,把示教点上的 shot_flags / offset_values / addr
|
||||
/// 映射成带理论时间和离散化时间的 ShotEvent,以及可直接注入伺服流的 TrajectoryDoEvent。
|
||||
/// </summary>
|
||||
public sealed class ShotTimelineBuilder
|
||||
{
|
||||
private readonly WaypointTimestampResolver _resolver;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ShotTimelineBuilder,依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
|
||||
/// </summary>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver)
|
||||
{
|
||||
_resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为给定轨迹构建完整的触发时间轴。
|
||||
/// </summary>
|
||||
/// <param name="trajectory">规划后的轨迹(含补中点信息和机器人配置)。</param>
|
||||
/// <param name="holdCycles">IO 保持周期数(对应原系统的 io_keep_cycles)。</param>
|
||||
/// <param name="samplePeriod">稠密采样周期,用于离散化 sample_index 和 sample_time。</param>
|
||||
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
if (holdCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(holdCycles), "IO 保持周期数不能为负数。");
|
||||
}
|
||||
|
||||
if (samplePeriod <= TimeSpan.Zero)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(samplePeriod), "采样周期必须大于零。");
|
||||
}
|
||||
|
||||
var timestamps = _resolver.Resolve(trajectory);
|
||||
var program = trajectory.OriginalProgram;
|
||||
var robot = trajectory.Robot;
|
||||
double triggerPeriodSeconds = robot.TriggerPeriod.TotalSeconds;
|
||||
double samplePeriodSeconds = samplePeriod.TotalSeconds;
|
||||
|
||||
var shotEvents = new List<ShotEvent>();
|
||||
var triggerTimeline = new List<TrajectoryDoEvent>();
|
||||
|
||||
for (int i = 0; i < program.Waypoints.Count; i++)
|
||||
{
|
||||
if (!program.ShotFlags[i])
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds;
|
||||
int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds);
|
||||
double sampleTime = sampleIndex * samplePeriodSeconds;
|
||||
|
||||
var addressGroup = program.AddressGroups[i];
|
||||
|
||||
shotEvents.Add(new ShotEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
sampleIndex: sampleIndex,
|
||||
sampleTime: sampleTime,
|
||||
addressGroup: addressGroup));
|
||||
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
offsetCycles: program.OffsetValues[i],
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup));
|
||||
}
|
||||
|
||||
return new ShotTimeline(shotEvents, triggerTimeline);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user