feat: 发布 EM 轨迹规划首个版本
This commit is contained in:
@@ -0,0 +1,60 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>将冻结的单方向段 EM 轨迹转换为几何控制器使用的 SI 单位有符号速度轨迹。</summary>
|
||||
public sealed class EmControlTrajectoryAdapter
|
||||
{
|
||||
private const double MinimumSegmentLengthMeters = 1e-6d;
|
||||
|
||||
/// <summary>按世界位置重建从零开始的弧长，同时保留航向、曲率以及前进为正倒车为负的参考速度。</summary>
|
||||
public Trajectory2D Create(EmTrajectory trajectory)
|
||||
{
|
||||
if (trajectory == null)
|
||||
throw new ArgumentNullException(nameof(trajectory));
|
||||
|
||||
var points = new List<TrajectoryPoint>(trajectory.Points.Count);
|
||||
double arcLengthMeters = 0d;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint source = trajectory.Points[index];
|
||||
var pose = new Pose2D(source.X, source.Y, source.Yaw);
|
||||
var converted = new TrajectoryPoint(arcLengthMeters, pose,
|
||||
source.VehicleCurvature, source.SignedLongitudinalVelocity);
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
points.Add(converted);
|
||||
continue;
|
||||
}
|
||||
|
||||
TrajectoryPoint previous = points[points.Count - 1];
|
||||
double deltaX = pose.XMeters - previous.PoseInWorld.XMeters;
|
||||
double deltaY = pose.YMeters - previous.PoseInWorld.YMeters;
|
||||
double distanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (distanceMeters < MinimumSegmentLengthMeters)
|
||||
{
|
||||
points[points.Count - 1] = new TrajectoryPoint(
|
||||
previous.ArcLengthMeters, pose, source.VehicleCurvature,
|
||||
source.SignedLongitudinalVelocity);
|
||||
continue;
|
||||
}
|
||||
|
||||
arcLengthMeters += distanceMeters;
|
||||
points.Add(new TrajectoryPoint(arcLengthMeters, pose,
|
||||
source.VehicleCurvature, source.SignedLongitudinalVelocity));
|
||||
}
|
||||
|
||||
if (points.Count < 2)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));
|
||||
}
|
||||
|
||||
return new Trajectory2D(points);
|
||||
}
|
||||
}
|
||||
+697
@@ -0,0 +1,697 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
using MyParking.Shared;
|
||||
using TrajectoryPlanningVisualization;
|
||||
using PlanningPose2D = MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>规划一次并冻结首个 EM 方向段，再使用现有几何控制器完成实车状态反馈闭环。</summary>
|
||||
[MovementTest(name = "EM闭环测试")]
|
||||
public sealed class EmClosedLoopMovementTest : MovementTest
|
||||
{
|
||||
private const int MaximumManualObstacleCount = 20;
|
||||
private static long nextObstacleSnapshotVersion;
|
||||
|
||||
public double GoalXmm = double.NaN;
|
||||
public double GoalYmm = double.NaN;
|
||||
public double GoalYawDeg;
|
||||
public double MapPaddingMeters = 2d;
|
||||
public float MapResolutionMm = 50f;
|
||||
public double SolverTimeoutSeconds = 5d;
|
||||
public int MaximumOsqpIterations = 100000;
|
||||
public double OutputTimeStepSeconds = 0.10d;
|
||||
public double VehicleLengthMeters = 0.80d;
|
||||
public double VehicleWidthMeters = 0.60d;
|
||||
public double SafetyMarginMeters = 0.05d;
|
||||
public double MaximumCurvaturePerMeter = 1d / 1.20d;
|
||||
public bool EnableWebVisualization = true;
|
||||
public bool AutoOpenWebVisualization = true;
|
||||
public int WebVisualizationPort;
|
||||
public double WebRefreshRateHz = 10d;
|
||||
public int VisualizationHistoryCycleLimit = 60;
|
||||
public bool EnableNativePainterVisualization = true;
|
||||
public double MaximumCommandSpeedMetersPerSecond = 1.00d;
|
||||
public double MaximumDistanceToTrajectoryMeters = 0.30d;
|
||||
public double ExecutionTimeoutSeconds = 120d;
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
/// <summary>读取冻结输入并启动一次规划、一次控制器接管的后台会话。</summary>
|
||||
public override void Test()
|
||||
{
|
||||
try
|
||||
{
|
||||
double goalXMillimeters = GoalXmm;
|
||||
double goalYMillimeters = GoalYmm;
|
||||
double goalYawDegrees = GoalYawDeg;
|
||||
if (!IsFinite(goalXMillimeters) || !IsFinite(goalYMillimeters))
|
||||
{
|
||||
goalXMillimeters = ReadFiniteInput("EM闭环终点 X（世界 mm）");
|
||||
goalYMillimeters = ReadFiniteInput("EM闭环终点 Y（世界 mm）");
|
||||
goalYawDegrees = ReadFiniteInput("EM闭环终点航向（世界 deg）");
|
||||
}
|
||||
|
||||
EnsureFinite(goalXMillimeters, nameof(GoalXmm));
|
||||
EnsureFinite(goalYMillimeters, nameof(GoalYmm));
|
||||
EnsureFinite(goalYawDegrees, nameof(GoalYawDeg));
|
||||
EnsurePositive(MaximumCommandSpeedMetersPerSecond,
|
||||
nameof(MaximumCommandSpeedMetersPerSecond));
|
||||
EnsurePositive(MaximumDistanceToTrajectoryMeters,
|
||||
nameof(MaximumDistanceToTrajectoryMeters));
|
||||
EnsurePositive(ExecutionTimeoutSeconds, nameof(ExecutionTimeoutSeconds));
|
||||
EnsurePositive(WheelAlignmentToleranceDegrees,
|
||||
nameof(WheelAlignmentToleranceDegrees));
|
||||
|
||||
var settings = new TrajectoryObservationSettings
|
||||
{
|
||||
PlanningScope = EmPlanningScope.FullDirectionSegment,
|
||||
MapPaddingMeters = MapPaddingMeters,
|
||||
MapResolutionMillimeters = MapResolutionMm,
|
||||
SolverTimeoutSeconds = SolverTimeoutSeconds,
|
||||
MaximumOsqpIterations = MaximumOsqpIterations,
|
||||
OutputTimeStepSeconds = OutputTimeStepSeconds,
|
||||
VehicleLengthMeters = VehicleLengthMeters,
|
||||
VehicleWidthMeters = VehicleWidthMeters,
|
||||
SafetyMarginMeters = SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
|
||||
EnableWebVisualization = EnableWebVisualization,
|
||||
AutoOpenWebVisualization = AutoOpenWebVisualization,
|
||||
WebVisualizationPort = WebVisualizationPort,
|
||||
WebRefreshRateHz = WebRefreshRateHz,
|
||||
VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
|
||||
EnableNativePainterVisualization = EnableNativePainterVisualization,
|
||||
}.CreateValidatedSnapshot();
|
||||
|
||||
IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
|
||||
long obstacleSnapshotVersion = obstacles.Count == 0
|
||||
? 0L
|
||||
: Interlocked.Increment(ref nextObstacleSnapshotVersion);
|
||||
var goal = new PlanningPose2D(
|
||||
goalXMillimeters / 1000d,
|
||||
goalYMillimeters / 1000d,
|
||||
goalYawDegrees * Math.PI / 180d);
|
||||
|
||||
EmClosedLoopMovementTestRunner.Start(goal, settings, obstacles,
|
||||
obstacleSnapshotVersion, MaximumCommandSpeedMetersPerSecond,
|
||||
MaximumDistanceToTrajectoryMeters, ExecutionTimeoutSeconds,
|
||||
WheelAlignmentToleranceDegrees);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
EmClosedLoopMovementTestRunner.ShowFailure("测试未启动", exception);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>取消尚未完成的规划并停止正在运行的控制任务。</summary>
|
||||
public override void TestStop()
|
||||
{
|
||||
EmClosedLoopMovementTestRunner.Stop();
|
||||
}
|
||||
|
||||
private static IReadOnlyList<TrajectoryObservationObstacle> ReadManualObstacles()
|
||||
{
|
||||
int count = ReadBoundedIntegerInput("EM闭环手动障碍物数量（0-20）",
|
||||
0, MaximumManualObstacleCount);
|
||||
var obstacles = new List<TrajectoryObservationObstacle>(count);
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
string label = "障碍物 " + (index + 1).ToString(CultureInfo.InvariantCulture);
|
||||
int kind = ReadBoundedIntegerInput(label + " 类型（1=圆形，2=轴对齐矩形）", 1, 2);
|
||||
double centerXMillimeters = ReadFiniteInput(label + " 中心 X（世界 mm）");
|
||||
double centerYMillimeters = ReadFiniteInput(label + " 中心 Y（世界 mm）");
|
||||
if (kind == 1)
|
||||
{
|
||||
double radiusMillimeters = ReadPositiveFiniteInput(label + " 半径（mm）");
|
||||
obstacles.Add(TrajectoryObservationObstacle.Circle(
|
||||
centerXMillimeters, centerYMillimeters, radiusMillimeters));
|
||||
}
|
||||
else
|
||||
{
|
||||
double lengthXMillimeters = ReadPositiveFiniteInput(label + " X 方向长度（mm）");
|
||||
double widthYMillimeters = ReadPositiveFiniteInput(label + " Y 方向宽度（mm）");
|
||||
obstacles.Add(TrajectoryObservationObstacle.Rectangle(
|
||||
centerXMillimeters - lengthXMillimeters / 2d,
|
||||
centerXMillimeters + lengthXMillimeters / 2d,
|
||||
centerYMillimeters - widthYMillimeters / 2d,
|
||||
centerYMillimeters + widthYMillimeters / 2d));
|
||||
}
|
||||
}
|
||||
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
|
||||
{
|
||||
object raw = UI.GetInput(prompt);
|
||||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||||
int value;
|
||||
if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
|
||||
!int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
|
||||
{
|
||||
throw new ArgumentException("输入必须是整数：" + prompt);
|
||||
}
|
||||
|
||||
if (value < minimum || value > maximum)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static double ReadPositiveFiniteInput(string prompt)
|
||||
{
|
||||
double value = ReadFiniteInput(prompt);
|
||||
EnsurePositive(value, prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static double ReadFiniteInput(string prompt)
|
||||
{
|
||||
object raw = UI.GetInput(prompt);
|
||||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||||
double value;
|
||||
if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
|
||||
!double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
|
||||
{
|
||||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||||
}
|
||||
|
||||
EnsureFinite(value, prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static void EnsurePositive(double value, string name)
|
||||
{
|
||||
EnsureFinite(value, name);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(name, "输入必须是正数：" + name);
|
||||
}
|
||||
|
||||
private static void EnsureFinite(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value))
|
||||
throw new ArgumentException("输入必须是有限数字：" + name);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>拥有单次规划和单个控制任务的会话生命周期，并保证停止操作幂等。</summary>
|
||||
internal static class EmClosedLoopMovementTestRunner
|
||||
{
|
||||
private static readonly object SessionSync = new object();
|
||||
private static CancellationTokenSource activeCancellation;
|
||||
private static Task activeTask;
|
||||
private static DriveTask activeDriveTask;
|
||||
private static TrajectoryObservationPresentation activePresentation;
|
||||
private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
|
||||
private static long nextSessionId;
|
||||
private static long activeSessionId;
|
||||
private static long stateSequence;
|
||||
|
||||
internal static void Start(PlanningPose2D goal, TrajectoryObservationSettings settings,
|
||||
IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion,
|
||||
double maximumCommandSpeedMetersPerSecond,
|
||||
double maximumDistanceToTrajectoryMeters,
|
||||
double executionTimeoutSeconds,
|
||||
float wheelAlignmentToleranceDegrees)
|
||||
{
|
||||
if (goal == null) throw new ArgumentNullException(nameof(goal));
|
||||
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
||||
if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
|
||||
|
||||
var cancellation = new CancellationTokenSource();
|
||||
CancellationTokenSource previousCancellation;
|
||||
DriveTask previousDriveTask;
|
||||
TrajectoryObservationPresentation previousPresentation;
|
||||
TrajectoryObservationVisualizationPublisher previousWebPublisher;
|
||||
long sessionId;
|
||||
lock (SessionSync)
|
||||
{
|
||||
previousCancellation = activeCancellation;
|
||||
previousDriveTask = activeDriveTask;
|
||||
previousPresentation = activePresentation;
|
||||
previousWebPublisher = activeWebPublisher;
|
||||
activeCancellation = cancellation;
|
||||
activeDriveTask = null;
|
||||
activePresentation = null;
|
||||
activeWebPublisher = null;
|
||||
activeTask = null;
|
||||
activeSessionId = sessionId = ++nextSessionId;
|
||||
}
|
||||
|
||||
Cancel(previousCancellation);
|
||||
StopDriveTask(previousDriveTask);
|
||||
ClearPresentation(previousPresentation);
|
||||
StopWebPublisher(previousWebPublisher);
|
||||
PrintStatus("会话 " + sessionId.ToString(CultureInfo.InvariantCulture) +
|
||||
" 已启动；车辆运动前将只规划一次。");
|
||||
|
||||
Task task = Task.Run(() => RunSessionAsync(sessionId, goal, settings,
|
||||
obstacles, obstacleSnapshotVersion, maximumCommandSpeedMetersPerSecond,
|
||||
maximumDistanceToTrajectoryMeters, executionTimeoutSeconds,
|
||||
wheelAlignmentToleranceDegrees, cancellation.Token), cancellation.Token);
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId == sessionId)
|
||||
activeTask = task;
|
||||
}
|
||||
|
||||
_ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed),
|
||||
TaskScheduler.Default);
|
||||
}
|
||||
|
||||
internal static void Stop()
|
||||
{
|
||||
CancellationTokenSource cancellation;
|
||||
DriveTask driveTask;
|
||||
TrajectoryObservationPresentation presentation;
|
||||
TrajectoryObservationVisualizationPublisher webPublisher;
|
||||
lock (SessionSync)
|
||||
{
|
||||
cancellation = activeCancellation;
|
||||
driveTask = activeDriveTask;
|
||||
presentation = activePresentation;
|
||||
webPublisher = activeWebPublisher;
|
||||
activeCancellation = null;
|
||||
activeDriveTask = null;
|
||||
activePresentation = null;
|
||||
activeWebPublisher = null;
|
||||
activeTask = null;
|
||||
activeSessionId = 0L;
|
||||
}
|
||||
|
||||
Cancel(cancellation);
|
||||
StopDriveTask(driveTask);
|
||||
ClearPresentation(presentation);
|
||||
StopWebPublisher(webPublisher);
|
||||
PrintStatus("已请求取消规划并停止控制任务。");
|
||||
}
|
||||
|
||||
internal static void ShowFailure(string context, Exception exception)
|
||||
{
|
||||
PrintStatus(context + "：" + (exception == null ? "未知错误" : exception.Message));
|
||||
}
|
||||
|
||||
private static async Task RunSessionAsync(long sessionId, PlanningPose2D goal,
|
||||
TrajectoryObservationSettings settings,
|
||||
IReadOnlyList<TrajectoryObservationObstacle> obstacles,
|
||||
long obstacleSnapshotVersion,
|
||||
double maximumCommandSpeedMetersPerSecond,
|
||||
double maximumDistanceToTrajectoryMeters,
|
||||
double executionTimeoutSeconds,
|
||||
float wheelAlignmentToleranceDegrees,
|
||||
CancellationToken token)
|
||||
{
|
||||
token.ThrowIfCancellationRequested();
|
||||
VehicleMotionState initialState = ReadPlanningState();
|
||||
using var planningDeadline = new TrajectoryObservationPlanningDeadline(
|
||||
TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), token);
|
||||
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||
initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
|
||||
TrajectoryObservationBootstrapResult bootstrap =
|
||||
new TrajectoryObservationBootstrapper().Bootstrap(job, planningDeadline, token);
|
||||
if (!bootstrap.Succeeded)
|
||||
throw new InvalidOperationException(bootstrap.FailureReason);
|
||||
|
||||
var controller = new TrajectoryObservationController(
|
||||
bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
|
||||
"em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
|
||||
PlanningCycleResult cycle = await controller.StartCycle(
|
||||
initialState.CapturedAtUtc, initialState, planningDeadline, token).ConfigureAwait(false);
|
||||
if (!cycle.Published || controller.PublishedTrajectory == null)
|
||||
{
|
||||
string reason = cycle.Result == null ? string.Empty : cycle.Result.FailureReason;
|
||||
throw new InvalidOperationException(string.IsNullOrWhiteSpace(reason)
|
||||
? cycle.Diagnostic
|
||||
: reason);
|
||||
}
|
||||
|
||||
EmTrajectory emTrajectory = controller.PublishedTrajectory;
|
||||
Trajectory2D controlTrajectory = new EmControlTrajectoryAdapter().Create(emTrajectory);
|
||||
TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualization(
|
||||
sessionId, bootstrap, controller, settings, obstacleSnapshotVersion, token);
|
||||
TrajectoryObservationPresentation presentation = settings.EnableNativePainterVisualization
|
||||
? new TrajectoryObservationPresentation()
|
||||
: null;
|
||||
if (!TryRegisterPresentation(sessionId, presentation, token))
|
||||
{
|
||||
ClearPresentation(presentation);
|
||||
token.ThrowIfCancellationRequested();
|
||||
return;
|
||||
}
|
||||
UpdateVisualization(sessionId, presentation, bootstrap, controller, emTrajectory,
|
||||
initialState, cycle, "规划已冻结，尚未向底盘下发控制指令。");
|
||||
PrintStatus("规划完成：EM点数=" + emTrajectory.Points.Count.ToString(CultureInfo.InvariantCulture) +
|
||||
"，控制点数=" + controlTrajectory.Count.ToString(CultureInfo.InvariantCulture) +
|
||||
"，长度=" + controlTrajectory.TotalLengthMeters.ToString("F3", CultureInfo.InvariantCulture) +
|
||||
" m，方向=" + emTrajectory.Metadata.Direction +
|
||||
"，终端=" + emTrajectory.Metadata.TerminalType + "。");
|
||||
|
||||
token.ThrowIfCancellationRequested();
|
||||
EmClosedLoopWheelSafety.RequireWheelsForward(wheelAlignmentToleranceDegrees);
|
||||
var movement = new TrajectoryTrackingMovement
|
||||
{
|
||||
Trajectory = controlTrajectory,
|
||||
MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
|
||||
MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
|
||||
ExecutionTimeoutSeconds = executionTimeoutSeconds,
|
||||
CycleObserver = control => UpdatePresentationFromControlCycle(
|
||||
sessionId, presentation, bootstrap, controller, emTrajectory, cycle, control),
|
||||
};
|
||||
var driveTask = new DriveTask(movement.Get());
|
||||
if (!TryRegisterDriveTask(sessionId, driveTask, token))
|
||||
{
|
||||
StopDriveTask(driveTask);
|
||||
token.ThrowIfCancellationRequested();
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
PrintStatus("冻结轨迹已交给现有控制器，开始单方向段闭环跟踪。");
|
||||
driveTask.Wait();
|
||||
if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch)
|
||||
PrintStatus("已在换向边界停车；本测试不启动下一方向段。");
|
||||
else
|
||||
PrintStatus("首个方向段已完成并停车。");
|
||||
}
|
||||
finally
|
||||
{
|
||||
StopDriveTask(driveTask);
|
||||
ClearDriveTask(sessionId, driveTask);
|
||||
}
|
||||
}
|
||||
|
||||
private static void UpdatePresentationFromControlCycle(long sessionId,
|
||||
TrajectoryObservationPresentation presentation,
|
||||
TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationController planningController,
|
||||
EmTrajectory trajectory,
|
||||
PlanningCycleResult cycle,
|
||||
MultiWheelC.Control.Execution.ParkingGeometricController control)
|
||||
{
|
||||
if (control == null || !control.LastVehicleState.HasValue)
|
||||
return;
|
||||
|
||||
var state = control.LastVehicleState.Value;
|
||||
var planningState = new VehicleMotionState(
|
||||
new PlanningPose2D(state.PoseInWorld.XMeters, state.PoseInWorld.YMeters,
|
||||
state.PoseInWorld.YawRadians),
|
||||
state.TwistInBody.VxMetersPerSecond, null, DateTimeOffset.UtcNow,
|
||||
Interlocked.Increment(ref stateSequence));
|
||||
string diagnostic = "现有控制器正在执行冻结轨迹";
|
||||
if (control.LastCommand.HasValue)
|
||||
{
|
||||
var command = control.LastCommand.Value;
|
||||
diagnostic += "：底盘速度=" +
|
||||
command.SpeedMetersPerSecond.ToString("F3", CultureInfo.InvariantCulture) +
|
||||
" m/s，前GCP=" +
|
||||
(command.FrontAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) +
|
||||
" deg，后GCP=" +
|
||||
(command.RearAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) + " deg。";
|
||||
}
|
||||
|
||||
UpdateVisualization(sessionId, presentation, bootstrap, planningController,
|
||||
trajectory, planningState, cycle, diagnostic);
|
||||
}
|
||||
|
||||
private static void UpdateVisualization(long sessionId,
|
||||
TrajectoryObservationPresentation presentation,
|
||||
TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationController controller,
|
||||
EmTrajectory trajectory,
|
||||
VehicleMotionState state,
|
||||
PlanningCycleResult cycle,
|
||||
string diagnostic)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId)
|
||||
return;
|
||||
var observation = new TrajectoryObservationObservation(
|
||||
DateTimeOffset.UtcNow, state, trajectory, null, null, null);
|
||||
DirectionSegmentView segment = controller.ActiveSegment;
|
||||
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
|
||||
trajectory, segment,
|
||||
controller.CreateEffectiveConfigurationSnapshot().Frenet.MaximumProjectionDistanceMeters);
|
||||
if (presentation != null && ReferenceEquals(activePresentation, presentation))
|
||||
{
|
||||
presentation.DrawWorld(bootstrap, observation,
|
||||
TrajectoryObservationRuntimeState.Create(DateTimeOffset.UtcNow, trajectory), diagnostic);
|
||||
presentation.DrawLs(charts, diagnostic);
|
||||
presentation.DrawSt(charts, diagnostic);
|
||||
}
|
||||
|
||||
TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher;
|
||||
if (publisher != null)
|
||||
{
|
||||
var tick = new TrajectoryObservationLoopTick(observation, cycle, TimeSpan.Zero,
|
||||
false, false, true, false, controller.SegmentState);
|
||||
publisher.TryPublish(observation.ObservedAtUtc, () =>
|
||||
{
|
||||
PlanningVisualizationDynamicSnapshot source =
|
||||
new TrajectoryObservationDynamicSnapshotBuilder().Build(
|
||||
Interlocked.Increment(ref stateSequence), tick, segment,
|
||||
controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
|
||||
controller.CreateEffectiveConfigurationSnapshot());
|
||||
return new PlanningVisualizationDynamicSnapshot(source.Sequence,
|
||||
source.ObservedAtUtc, "EM闭环控制中", source.ActiveSegmentIndex,
|
||||
source.ActiveDirection, source.VehiclePose, source.DynamicPolylines,
|
||||
source.DynamicMarkers, source.Charts, source.StatusValues, source.CycleSummary);
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static TrajectoryObservationVisualizationPublisher StartWebVisualization(
|
||||
long sessionId, TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationController controller, TrajectoryObservationSettings settings,
|
||||
long obstacleSnapshotVersion, CancellationToken token)
|
||||
{
|
||||
if (!settings.EnableWebVisualization)
|
||||
return null;
|
||||
var publisher = new TrajectoryObservationVisualizationPublisher(settings,
|
||||
new PlanningVisualizationSessionSink(), PrintStatus);
|
||||
try
|
||||
{
|
||||
PlanningVisualizationStaticSnapshot source =
|
||||
new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
|
||||
controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion);
|
||||
var closedLoopSnapshot = new PlanningVisualizationStaticSnapshot("EM闭环测试",
|
||||
source.WorldBounds, source.OccupancyGrid, source.StaticPolylines,
|
||||
source.StaticMarkers, source.DirectionSegments, source.ConfigurationGroups);
|
||||
PlanningVisualizationSessionInfo info = publisher.Start(closedLoopSnapshot);
|
||||
if (info == null)
|
||||
return null;
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId || token.IsCancellationRequested)
|
||||
{
|
||||
publisher.Stop();
|
||||
return null;
|
||||
}
|
||||
activeWebPublisher = publisher;
|
||||
}
|
||||
PrintStatus("网页可视化地址（含会话令牌）：" + info.Uri.AbsoluteUri);
|
||||
if (settings.AutoOpenWebVisualization)
|
||||
{
|
||||
Process.Start(new ProcessStartInfo
|
||||
{
|
||||
FileName = info.Uri.AbsoluteUri,
|
||||
UseShellExecute = true,
|
||||
});
|
||||
}
|
||||
return publisher;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
publisher.Disable(exception);
|
||||
PrintStatus("网页可视化未启动：" + exception.Message);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryRegisterPresentation(long sessionId,
|
||||
TrajectoryObservationPresentation presentation, CancellationToken token)
|
||||
{
|
||||
if (presentation == null)
|
||||
return true;
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId || activeCancellation == null ||
|
||||
activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
|
||||
return false;
|
||||
activePresentation = presentation;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private static VehicleMotionState ReadPlanningState()
|
||||
{
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (location == null)
|
||||
throw new InvalidOperationException("实时定位不可用。");
|
||||
if (BasicPilotBase.Chassis == null)
|
||||
throw new InvalidOperationException("实时底盘读接口不可用。");
|
||||
|
||||
var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
|
||||
return new VehicleMotionState(
|
||||
new PlanningPose2D(location.x / 1000d, location.y / 1000d,
|
||||
location.th * Math.PI / 180d),
|
||||
speed.Vx, null, DateTimeOffset.UtcNow,
|
||||
Interlocked.Increment(ref stateSequence));
|
||||
}
|
||||
|
||||
private static bool TryRegisterDriveTask(long sessionId, DriveTask driveTask,
|
||||
CancellationToken token)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId || activeCancellation == null ||
|
||||
activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
activeDriveTask = driveTask;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private static void ClearDriveTask(long sessionId, DriveTask driveTask)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId == sessionId && ReferenceEquals(activeDriveTask, driveTask))
|
||||
activeDriveTask = null;
|
||||
ClearPresentation(activePresentation);
|
||||
activePresentation = null;
|
||||
StopWebPublisher(activeWebPublisher);
|
||||
activeWebPublisher = null;
|
||||
}
|
||||
}
|
||||
|
||||
private static void Finish(long sessionId, CancellationTokenSource cancellation,
|
||||
Task completed)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId == sessionId)
|
||||
{
|
||||
activeCancellation = null;
|
||||
activeDriveTask = null;
|
||||
activeTask = null;
|
||||
activeSessionId = 0L;
|
||||
}
|
||||
}
|
||||
|
||||
if (completed.IsFaulted)
|
||||
{
|
||||
Exception failure = completed.Exception == null
|
||||
? null
|
||||
: completed.Exception.GetBaseException();
|
||||
ShowFailure("闭环会话失败并已停车", failure);
|
||||
}
|
||||
else if (completed.IsCanceled)
|
||||
{
|
||||
PrintStatus("闭环会话已取消。");
|
||||
}
|
||||
|
||||
cancellation.Dispose();
|
||||
}
|
||||
|
||||
private static void Cancel(CancellationTokenSource cancellation)
|
||||
{
|
||||
if (cancellation == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
cancellation.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private static void StopDriveTask(DriveTask driveTask)
|
||||
{
|
||||
if (driveTask == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
driveTask.Stop();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowFailure("停止控制任务失败", exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static void ClearPresentation(TrajectoryObservationPresentation presentation)
|
||||
{
|
||||
if (presentation == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
presentation.ClearAll();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowFailure("清理闭环可视化失败", exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static void StopWebPublisher(TrajectoryObservationVisualizationPublisher publisher)
|
||||
{
|
||||
if (publisher == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
publisher.Stop();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowFailure("停止闭环网页可视化失败", exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static void PrintStatus(string message)
|
||||
{
|
||||
Console.WriteLine("[EM闭环测试] " + message);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>在控制器接管底盘前只读检查四个舵轮是否已经与车体前向对齐。</summary>
|
||||
internal static class EmClosedLoopWheelSafety
|
||||
{
|
||||
internal static void RequireWheelsForward(float toleranceDegrees)
|
||||
{
|
||||
var chassis = PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
throw new InvalidOperationException("当前底盘不是 MultiWheelChassis。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(chassis, PilotDefinition.Self.CarNum);
|
||||
double toleranceRadians = AngleMath.DegreesToRadians(toleranceDegrees);
|
||||
if (!adapter.AreParallelWheelsAligned(0d, toleranceRadians))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"四个舵轮尚未与车头方向一致，控制器未接管底盘。");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,157 +1,201 @@
|
||||
# EM trajectory observation MovementTest
|
||||
# EM 轨迹规划 MovementTest（观察与闭环）
|
||||
|
||||
`TrajectoryObservationMovementTest` is an observe-only host for the real MDCS localization and chassis-speed read
|
||||
interfaces. It bootstraps the coarse path and Local G2 reference once, requests one frozen full-direction-segment EM
|
||||
plan per active direction segment by default, samples the latest published trajectory, and draws the world, LS, and ST
|
||||
layers. It does not drive, steer, brake, change gear, or invoke a geometric vehicle controller.
|
||||
`tarjplanner_movementtest` 将粗路径、Local G2 连续参考和 EM 轨迹规划接入两个 MovementTest：
|
||||
|
||||
Every runtime status contains `OBSERVE_ONLY: no chassis command is sent.` Treat the displayed control command as a
|
||||
diagnostic prediction only. Goal and rolling-safety-stop commands are logged, never applied to hardware. At the end of a
|
||||
gear-switch trajectory, the observer remains on the current direction segment and waits for real direction confirmation;
|
||||
it does not create or dispatch a direction-change action.
|
||||
- `EM轨迹规划观察闭环测试`：只读地采样真实定位和底盘速度，规划并观察轨迹，**绝不向底盘发送命令**。
|
||||
- `EM闭环测试`：冻结首个 EM 方向段后，把轨迹交给现有几何控制器执行；这是会实际控制车辆的测试，必须在受监督的安全场地使用。
|
||||
|
||||
Every frozen observation session writes a text report to
|
||||
`TrajectoryObservationReports/trajectory-observation-session-<id>.txt` in the process working directory when the
|
||||
session stops; the report path is printed to the console.
|
||||
两种测试都会在启动时冻结目标、手动障碍物、地图和规划配置。它们不读取或修改传感器地图，也不做持续重建图。观察测试的所有状态都包含 `OBSERVE_ONLY: no chassis command is sent.`；其中显示的控制命令只是诊断预测，不能转发到硬件。
|
||||
|
||||
## FullDirectionSegment observation (default)
|
||||
## 模块说明（Module Overview）
|
||||
|
||||
`UseFullDirectionSegmentPlanning=true` maps to `EmPlanningScope.FullDirectionSegment`. MovementTest performs one frozen
|
||||
optimization for the complete remaining active direction segment, then republishes that trajectory with fresh observation
|
||||
state until the real direction boundary is confirmed and the next segment becomes active. `s_end` is the actual Local G2
|
||||
`PathS` at the active segment boundary; `T_end` is derived from the feasible acceleration, cruise, jerk-limited stopping,
|
||||
and zero-speed hold behavior instead of being forced by a legacy time horizon.
|
||||
| 模块 | 负责内容 | 不负责内容 |
|
||||
| --- | --- | --- |
|
||||
| `Map` | 根据冻结的起点、终点和手动障碍物创建不可变规划地图 | 实时环境更新、车辆控制 |
|
||||
| `CoarsePath` | Hybrid A* 粗路径、碰撞复核与方向分段 | 速度规划、底盘命令 |
|
||||
| `PathSmoothing` | 将粗路径转换为连续的 Local G2 参考 | 实时状态读取、控制 |
|
||||
| `EMPlanner` | 针对活动方向段生成不可变 `EmTrajectory` | 定位读取、UI、硬件协议 |
|
||||
| `TrajectoryExecution` | 轨迹采样、换向状态和诊断性控制意图 | 底盘接口、命令下发 |
|
||||
| 本目录 | 会话编排、状态采样、可视化、报告，以及闭环测试的轨迹适配 | 改写上游规划结果或地图快照 |
|
||||
|
||||
`DistanceHorizonMeters` and `TimeHorizonSeconds` are rolling-compatible fields and do not truncate full mode. Forward
|
||||
desired/hard maximum speed is `1.0 m/s`; reverse desired/hard maximum speed is `0.5 m/s`. A successful real boundary
|
||||
requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`).
|
||||
`EM轨迹规划观察闭环测试` 在换向边界只等待真实方向确认；不创建、不派发换向动作。`EM闭环测试` 只执行冻结的首个方向段；若终端是换向边界，它停车后不会自动启动下一段。
|
||||
|
||||
## Rolling trajectory observation
|
||||
## 文件结构（File Structure）
|
||||
|
||||
Rolling trajectory observation remains `OBSERVE_ONLY` and never sends a chassis command. Rolling and approach trajectories
|
||||
may end with nonzero speed because `DistanceHorizonMeters` is an L-S reference window and `TimeHorizonSeconds` is the
|
||||
single S-T output duration. Only a real Goal or gear-switch boundary may publish an exact zero-speed terminal.
|
||||
```text
|
||||
tarjplanner_movementtest/
|
||||
├── README.md # 模块边界、配置、操作和限制
|
||||
├── MovementTest.TrajectoryObservationTest.cs # 只读观察 UI 入口与会话生命周期
|
||||
├── MovementTest.EmClosedLoopTest.cs # 实际闭环 UI 入口、舵轮前向校验与停止
|
||||
├── EmControlTrajectoryAdapter.cs # EM 单方向段轨迹转几何控制器 Trajectory2D
|
||||
├── TrajectoryObservationContracts.cs # 设置快照、障碍物、启动输入和结果契约
|
||||
├── TrajectoryObservationPipeline.cs # Bootstrap、EM 周期、采样和观察循环
|
||||
├── TrajectoryObservationPlanningDeadline.cs # 单周期截止时间与发布授权
|
||||
├── TrajectoryObservationSegmentTracker.cs # 终停、真实换向确认和活动方向段推进
|
||||
├── TrajectoryObservationStaticSnapshotBuilder.cs # 地图、路径、配置的静态网页快照
|
||||
├── TrajectoryObservationDynamicSnapshotBuilder.cs # 每个 tick 的动态 World/LS/ST 快照
|
||||
├── TrajectoryObservationKinematicChartBuilder.cs # 速度、加速度、jerk、曲率等图表数据
|
||||
├── TrajectoryObservationHandoffAnalyzer.cs # 轨迹交接与连续性诊断
|
||||
├── TrajectoryObservationDiagnostics.cs # 配置、规划和运行诊断模型
|
||||
├── TrajectoryObservationPresentation.cs # Painter 图层与中文状态文本
|
||||
├── TrajectoryObservationVisualizationPublisher.cs # 本机网页会话与 Painter 发布隔离
|
||||
└── TrajectoryObservationReportWriter.cs # 只读观察会话文本报告
|
||||
```
|
||||
|
||||
## Configuration
|
||||
## 运行数据流（Runtime Data Flow）
|
||||
|
||||
| Field | Unit | MovementTest UI default | Meaning |
|
||||
```text
|
||||
目标、手动障碍物、实时起始位姿和冻结设置
|
||||
│
|
||||
▼
|
||||
Map -> CoarsePath -> PathSmoothing
|
||||
│
|
||||
▼
|
||||
当前方向段的 EMPlanner -> EmTrajectory
|
||||
│
|
||||
┌─────────┴──────────────────┐
|
||||
▼ ▼
|
||||
只读观察：每个 tick 读取状态 闭环测试：首段 Trajectory2D
|
||||
并采样/绘制/报告 -> 现有几何控制器 -> 底盘
|
||||
```
|
||||
|
||||
观察测试中，规划失败、报告写入失败、网页或 Painter 发布失败均只产生诊断，不能改变其只读属性。闭环测试在控制器接管前检查底盘类型和四个舵轮是否与车体前向平行；不满足时不会接管底盘。
|
||||
|
||||
## 运行状态与停止（Runtime State and Stop）
|
||||
|
||||
每个测试同时最多保留一个活动会话。重新启动会取消前一会话；`TestStop()` 取消规划和后台任务，并清理 Painter 与网页发布器。
|
||||
|
||||
观察测试在停止时把会话内容写入进程工作目录的：
|
||||
|
||||
```text
|
||||
TrajectoryObservationReports/trajectory-observation-session-<id>.txt
|
||||
```
|
||||
|
||||
报告路径会输出到控制台。关闭浏览器标签不会停止会话；必须使用 MovementTest 的正常停止操作。闭环测试停止时会请求停止当前 `DriveTask`，并清理其可视化资源。
|
||||
|
||||
## 坐标与单位（Coordinates and Units）
|
||||
|
||||
| 项目 | 坐标/单位 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| UI 目标与手动障碍物 | 世界坐标 `mm`、航向 `deg` | 入口处转换为规划用 SI 单位。 |
|
||||
| `Pose2D` / EM 路径 | `m`、`rad` | 车辆几何中心的世界位姿。 |
|
||||
| `ReferenceS` | `m` | 投影到完整活动方向段后的共享参考站。 |
|
||||
| `PathS` | `m` | ST 求解使用、从活动段局部起点累计的弧长；不可与 `ReferenceS` 跨段直接相减。 |
|
||||
| `l`、`v`、`a`、`j`、`κ`、`ω` | `m`、`m/s`、`m/s²`、`m/s³`、`m⁻¹`、`rad/s` | 网页/Painter 图表使用的横纵轴单位。 |
|
||||
|
||||
`EmTrajectoryPoint.VelocityX` 与 `VelocityY` 是世界坐标系分量。观察测试不会进行驱动坐标转换，也不会把它们发送到车辆接口。闭环测试使用 `EmControlTrajectoryAdapter`，以世界位置重建弧长并保留有符号纵向速度、航向和曲率，得到现有控制器使用的 `Trajectory2D`。
|
||||
|
||||
## 最小使用说明（Minimal Use）
|
||||
|
||||
### 只读观察
|
||||
|
||||
1. 在车辆 UI 选择 `EM轨迹规划观察闭环测试`。
|
||||
2. 设置有限的 `GoalXmm`、`GoalYmm`、`GoalYawDeg`；X 或 Y 为 `NaN` 时会依次弹出输入框。
|
||||
3. 输入 `0` 至 `20` 个手动障碍物，或输入 `0` 使用空障碍物快照。
|
||||
4. 启动测试，确认状态含有 `OBSERVE_ONLY: no chassis command is sent.`。
|
||||
5. 查看网页或 Painter 的 World、LS、ST 与运动学图层；停止时使用 UI 的正常停止操作。
|
||||
|
||||
### 实车闭环
|
||||
|
||||
1. 仅在已审查、受监督的车辆和场地中选择 `EM闭环测试`。
|
||||
2. 确认实时定位与底盘读接口可用；确认底盘为 `MultiWheelChassis`，且四个舵轮已对齐车头方向。
|
||||
3. 设置目标、障碍物以及命令速度、偏离阈值和执行超时。
|
||||
4. 启动后先等待“规划完成”状态；只有通过舵轮校验后，冻结的首个方向段才会交给现有控制器。
|
||||
5. 任意异常或人工停止时，使用 `TestStop()` 所对应的 UI 停止动作；确认控制任务已停止。
|
||||
|
||||
## 配置（Configuration）
|
||||
|
||||
### 两个入口共有的规划与可视化配置
|
||||
|
||||
| 字段 | 单位 | UI 默认值 | 含义 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `GoalXmm` | world mm | `NaN` | Goal X. If X or Y is not finite, the host prompts for X, Y, and yaw. |
|
||||
| `GoalYmm` | world mm | `NaN` | Goal Y. |
|
||||
| `GoalYawDeg` | world deg | `0` | Goal heading. |
|
||||
| `MapPaddingMeters` | m | `2.0` | Padding added on all sides of the start/goal bounds. |
|
||||
| `MapResolutionMm` | mm | `50` | Local occupancy-grid resolution. |
|
||||
| `UseFullDirectionSegmentPlanning` | bool | `true` | Selects `FullDirectionSegment`; set `false` for legacy `RollingHorizon`. |
|
||||
| `ReplanPeriodSeconds` | s | `0.20` | Minimum interval between EM planning cycles in rolling scope. |
|
||||
| `ObserverPeriodSeconds` | s | `0.05` | Live-state sampling and redraw interval. |
|
||||
| `SolverTimeoutSeconds` | s | `5.0` | EM solver timeout frozen for the session. |
|
||||
| `MaximumOsqpIterations` | iterations | `100000` | OSQP iteration limit frozen for the session. |
|
||||
| `TimeHorizonSeconds` | s | `20.0` | Rolling-only ST trajectory horizon, not the observer period; full mode derives `T_end` instead. |
|
||||
| `OutputTimeStepSeconds` | s | `0.10` | Published trajectory timestamp spacing. |
|
||||
| `VehicleLengthMeters` | m | `0.80` | Vehicle envelope length supplied to coarse, smoothing, and EM planning. |
|
||||
| `VehicleWidthMeters` | m | `0.60` | Vehicle envelope width supplied to planning. |
|
||||
| `SafetyMarginMeters` | m | `0.05` | Additional planning clearance outside the vehicle envelope. |
|
||||
| `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | Maximum allowed vehicle curvature (about `0.8333 1/m`). |
|
||||
| `EnableWebVisualization` | bool | `true` | Enables the primary local web dashboard session. |
|
||||
| `AutoOpenWebVisualization` | bool | `true` | Opens the tokenized local dashboard URI when web output is enabled. |
|
||||
| `WebVisualizationPort` | port | `0` | `0` selects an ephemeral port; otherwise use `1024` through `65535`. |
|
||||
| `WebRefreshRateHz` | Hz | `10` | Maximum dynamic web snapshot publication rate. |
|
||||
| `VisualizationHistoryCycleLimit` | cycles | `60` | Bounded web history length. |
|
||||
| `EnableNativePainterVisualization` | bool | `true` | Enables optional native Painter audit windows; disable unless explicitly reviewed. |
|
||||
| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | Minimum signed-speed magnitude used to confirm the next direction. |
|
||||
| `DirectionConfirmationSamples` | samples | `3` | Strictly increasing matching-direction samples required after the stop hold. |
|
||||
| `GearSwitchProjectionToleranceMeters` | m | `0.50` | Maximum switch-point projection distance for both adjacent segments. |
|
||||
| `GearSwitchStopHoldSeconds` | s | `0.20` | Continuous real-stop duration required before direction samples count. |
|
||||
| `GoalXmm` / `GoalYmm` | 世界 mm | `NaN` | 终点位置；非有限值时弹出输入。 |
|
||||
| `GoalYawDeg` | 世界 deg | `0` | 终点航向。 |
|
||||
| `MapPaddingMeters` | m | `2.0` | 起点/终点范围四周增加的地图边距。 |
|
||||
| `MapResolutionMm` | mm | `50` | 局部占据栅格分辨率。 |
|
||||
| `SolverTimeoutSeconds` | s | `5.0` | 冻结的单次规划截止时间。 |
|
||||
| `MaximumOsqpIterations` | 次 | `100000` | OSQP 最大迭代数。 |
|
||||
| `OutputTimeStepSeconds` | s | `0.10` | 发布轨迹相邻时间戳间隔。 |
|
||||
| `VehicleLengthMeters` / `VehicleWidthMeters` | m | `0.80` / `0.60` | 规划使用的车辆外形尺寸。 |
|
||||
| `SafetyMarginMeters` | m | `0.05` | 车辆外形之外的额外安全余量。 |
|
||||
| `MaximumCurvaturePerMeter` | `m⁻¹` | `1 / 1.20` | 允许的最大曲率。 |
|
||||
| `EnableWebVisualization` | bool | `true` | 启用本机网页看板。 |
|
||||
| `AutoOpenWebVisualization` | bool | `true` | 网页会话启动后尝试打开浏览器。 |
|
||||
| `WebVisualizationPort` | port | `0` | `0` 自动选择回环端口；否则必须为 `1024` 至 `65535`。 |
|
||||
| `WebRefreshRateHz` | Hz | `10` | 动态网页快照的最高发布频率。 |
|
||||
| `VisualizationHistoryCycleLimit` | cycles | `60` | 网页保存的有限历史周期数。 |
|
||||
| `EnableNativePainterVisualization` | bool | `true` | 启用可选 Painter 图层。 |
|
||||
|
||||
`TrajectoryObservationSettings` contract defaults are `PlanningScope=FullDirectionSegment`,
|
||||
`TimeHorizonSeconds=2.0`, `EnableWebVisualization=true`, and `EnableNativePainterVisualization=false`; the MovementTest
|
||||
UI currently exposes the values above. All public fields are validated and copied into a frozen input snapshot before
|
||||
the background session starts. Later edits cannot change an active session. The live pose and actual longitudinal speed
|
||||
are then read once per observer tick.
|
||||
`TrajectoryObservationSettings` 的契约默认值与 UI 不完全相同：`TimeHorizonSeconds=2.0`、`EnableNativePainterVisualization=false`，其余入口字段由各 MovementTest UI 显式赋值。启动前调用 `CreateValidatedSnapshot()`；之后 UI 改动不会影响活动会话。
|
||||
|
||||
In full scope, EM planning runs once per active segment with one attempt in flight. In rolling scope, EM planning
|
||||
respects `ReplanPeriodSeconds` with at most one planning cycle in flight. Every observer tick still captures fresh
|
||||
state, samples the currently published trajectory, redraws all layers, and emits a session-guarded status; a slow
|
||||
planner therefore does not reduce the configured observation cadence.
|
||||
### 只读观察专用配置
|
||||
|
||||
## Manual obstacles
|
||||
| 字段 | 单位 | UI 默认值 | 含义 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `UseFullDirectionSegmentPlanning` | bool | `true` | `true` 使用 `FullDirectionSegment`，`false` 使用 `RollingHorizon`。 |
|
||||
| `ReplanPeriodSeconds` | s | `0.20` | 滚动模式两次规划之间的最小间隔。 |
|
||||
| `ObserverPeriodSeconds` | s | `0.05` | 读取状态、采样、绘图的观察周期。 |
|
||||
| `TimeHorizonSeconds` | s | `20.0` | 仅滚动兼容字段；完整方向段模式不用于截断。 |
|
||||
| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | 换向确认所需的下一方向速度阈值。 |
|
||||
| `DirectionConfirmationSamples` | samples | `3` | 停车后连续有效方向样本数。 |
|
||||
| `GearSwitchProjectionToleranceMeters` | m | `0.50` | 换向点在相邻两段上的最大投影距离。 |
|
||||
| `GearSwitchStopHoldSeconds` | s | `0.20` | 方向样本开始计数前要求的连续停车保持时间。 |
|
||||
|
||||
Enter a count from `0` through `20`, then select each obstacle type. Coordinates are global/world millimeters.
|
||||
完整方向段模式对每个活动方向段仅规划一次；滚动模式遵守 `ReplanPeriodSeconds`。无论规划是否较慢，观察循环仍按 `ObserverPeriodSeconds` 读取新鲜状态并发布当前轨迹快照。
|
||||
|
||||
- Circle example: center `(2500, 1200) mm`, radius `300 mm`.
|
||||
- Axis-aligned rectangle example: center `(4000, -500) mm`, X length `800 mm`, Y width `500 mm`.
|
||||
### 闭环专用配置
|
||||
|
||||
The complete obstacle envelope must fit inside the start/goal bounds plus `MapPaddingMeters`; otherwise bootstrap is
|
||||
rejected before EM planning.
|
||||
| 字段 | 单位 | UI 默认值 | 含义 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `MaximumCommandSpeedMetersPerSecond` | m/s | `1.00` | 交给现有控制器的最大命令速度。 |
|
||||
| `MaximumDistanceToTrajectoryMeters` | m | `0.30` | 控制器允许的最大轨迹偏离距离。 |
|
||||
| `ExecutionTimeoutSeconds` | s | `120` | 首个方向段执行超时。 |
|
||||
| `WheelAlignmentToleranceDegrees` | deg | `2` | 四个舵轮与前向平行的最大角度误差。 |
|
||||
|
||||
## Reading the layers
|
||||
闭环入口固定使用 `FullDirectionSegment`，规划成功后只执行第一个活动方向段。
|
||||
|
||||
- `TrajectoryObserver.World` shows map bounds and occupied cells, the frozen start and goal, Hybrid A* coarse path,
|
||||
Local G2 smoothed path, current real pose, and latest published EM trajectory.
|
||||
- `TrajectoryObserver.LS` plots reference path-S horizontally and lateral offset vertically. Projection failures indicate
|
||||
trajectory points that could not be associated with the current direction segment.
|
||||
- `TrajectoryObserver.ST` overlays time-to-path-S and time-to-signed-speed. Use it to check monotonic time/progress,
|
||||
stop profiles, and the sign of forward/reverse velocity.
|
||||
## 手动障碍物与会话冻结（Obstacles and Frozen Session）
|
||||
|
||||
`EmTrajectoryPoint.VelocityX` and `VelocityY` are world-frame components. They are not chassis-frame velocity commands
|
||||
and must never be forwarded directly to a vehicle motion interface. This MovementTest performs no coordinate conversion
|
||||
for driving and has no driving capability; any future execution mode requires a separate safety-reviewed design.
|
||||
两个入口都要求输入 `0` 至 `20` 个障碍物，坐标均为世界 `mm`：
|
||||
|
||||
## 网页看板与可选 Painter
|
||||
- 圆形：中心 `(2500, 1200) mm`、半径 `300 mm`。
|
||||
- 轴对齐矩形：中心 `(4000, -500) mm`、X 向长度 `800 mm`、Y 向宽度 `500 mm`。
|
||||
|
||||
将 `EnableWebVisualization=true` 可启用本机网页看板；它只绑定 `127.0.0.1`，启动日志会给出完整的、带随机
|
||||
会话令牌的 URL。端口为 `0` 时由系统选择可用回环端口；浏览器必须使用该完整 URL，不能删掉 token。默认
|
||||
`AutoOpenWebVisualization=true` 会尝试打开默认浏览器；无桌面会话或打开失败只记录 URL，服务和观察循环继续运行。
|
||||
网页默认以 `10 Hz` 发布不可变动态快照，网页关闭、断开或慢客户端只会丢弃网页帧，绝不会阻塞、序列化等待或改变
|
||||
规划周期。
|
||||
障碍物完整包络必须位于起点/终点矩形加 `MapPaddingMeters` 后的地图范围内，否则启动阶段拒绝会话。启动成功后，目标、障碍物、地图、车辆参数和配置均为会话快照；不会跟随 UI 后续编辑改变。
|
||||
|
||||
`EnableNativePainterVisualization=true` 才会为本会话创建 `TrajectoryObserver.World`、`LS`、`ST` Painter。网页是主
|
||||
观察界面，Painter 仅作为可选的正确性/审计输出；受监督车辆清单执行前应显式关闭 Painter，除非另行审查。网页与
|
||||
Painter 可以同时开启，也可全部关闭：全部关闭时仍保留 `OBSERVE_ONLY` 规划、控制台和 UI 状态。网页的启动、快照或
|
||||
服务出现异常时会只记录一次中文诊断并熔断本次网页输出，不自动循环重启，也不会停止规划观察。
|
||||
## 网页看板与可选 Painter（Visualization）
|
||||
|
||||
网页采用中文说明配合科研绘图约定：白底、细灰网格和细曲线；当前轨迹为蓝色实线，上一轮为灰色虚线，换向与交接点
|
||||
为橙色，红色仅表示实际越界或失败。路径总览把已完成方向段画为灰色细实线、活动段画为蓝色细实线、未来段画为浅灰
|
||||
虚线，并以深蓝覆盖当前规划视界。页面的 `ReferenceS (m)` 是将轨迹投影到**完整活动方向段**后的共享参考站；
|
||||
`PathS (m)` 是 ST 求解使用、从该段局部起点累计的实际路径弧长，二者不可互相替代或跨段直接相减。
|
||||
启用 `EnableWebVisualization=true` 后，服务只绑定 `127.0.0.1`。启动日志输出含随机会话令牌的完整 URL；必须使用完整地址，不能去掉 token。端口为 `0` 时系统选择可用回环端口。浏览器打开失败、客户端断开或快照发布异常只记录诊断并隔离该网页输出，不应阻塞规划、观察或闭环停止。
|
||||
|
||||
所有图表轴标签与单位由实际快照提供：世界 `X/Y (m)`、`ReferenceS (m)`、`PathS (m)`、`t (s)`、`l (m)`、
|
||||
`v (m/s)`、`a (m/s²)`、`j (m/s³)`、`κ (m⁻¹)`、`ω (rad/s)`。`ls` 的横轴是 `ReferenceS (m)`、纵轴是 `l (m)`；
|
||||
`st` 的横轴是 `t (s)`、纵轴是 `PathS (m)`。每张图支持鼠标滚轮以指针为中心缩放、拖拽框选局部放大、`重置视图`
|
||||
与 `全屏`；这些操作只改变本地视口，不修改原始快照，也不向规划器发送参数。
|
||||
启用 `EnableNativePainterVisualization=true` 后，可查看 `TrajectoryObserver.World`、`TrajectoryObserver.LS` 与 `TrajectoryObserver.ST` 图层。网页是主观察界面；Painter 是可选审计输出。网页与 Painter 可同时启用，也可同时关闭。
|
||||
|
||||
运动学页中 `j[i]` 仅表示真实区间 `[tᵢ, tᵢ₊₁)`，所以 jerk 只有 `N-1` 个样本；末点之后没有虚构的 `j=0`。
|
||||
状态页将滚动续航、接近停车边界和精确停车边界分别标为 `RollingContinuation`、
|
||||
`ApproachStopBoundary`、`ExactStopAtBoundary`：前两者允许非零末端速度，只有真实边界才是精确停车锚点。换向高亮
|
||||
仅会在投影位置满足容差、已连续停车保持 `GearSwitchStopHoldSeconds`，并取得
|
||||
`DirectionConfirmationSamples` 个满足 `DirectionConfirmationSpeedMetersPerSecond` 阈值的下一方向带符号速度样本后，
|
||||
严格从 `N -> N+1` 前进。
|
||||
- World：地图边界、占据格、冻结起点/终点、粗路径、Local G2、实时位姿和 EM 轨迹。
|
||||
- LS：横轴 `ReferenceS (m)`，纵轴 `l (m)`；投影失败表示点无法关联到活动方向段。
|
||||
- ST：横轴 `t (s)`，纵轴 `PathS (m)`；用于检查时间、进度、停车与速度符号。
|
||||
- 运动学图：显示 `v`、`a`、`j`、`κ`、`ω` 等快照数据。`j[i]` 对应区间 `[tᵢ, tᵢ₊₁)`，因此只有 `N-1` 个样本。
|
||||
|
||||
## Operator launch and stop checklist
|
||||
## 报告与诊断（Reports and Diagnostics）
|
||||
|
||||
The vehicle UI entry is `EM轨迹规划观察闭环测试`. It is an observation session only; it has no chassis,
|
||||
motor, steering, brake, or gear output.
|
||||
只读观察会话把带时间戳的诊断写入文本报告，包含会话编号、运行状态和 `OBSERVE_ONLY` 安全标识。常见状态包括当前活动方向段、规划是否已发布、轨迹采样结果、换向确认进度和可视化地址。
|
||||
|
||||
Before starting:
|
||||
闭环测试会在控制器接管前输出冻结轨迹点数、控制点数、长度、方向和终端类型；执行期间输出底盘速度与前后 GCP 角度，供监督人员诊断。该测试的命令输出属于真实控制流程，不能按观察模式解释。
|
||||
|
||||
1. Ensure the vehicle is in a safe, supervised state; this test reads live localization and signed chassis speed but
|
||||
never controls the vehicle.
|
||||
2. Confirm localization and the chassis read interface are available. The session reports a startup failure if either
|
||||
cannot be read.
|
||||
3. Set a finite goal X/Y/yaw (or be ready to enter them when prompted), then enter zero to twenty manual obstacles.
|
||||
Each obstacle must remain inside the configured start/goal rectangle plus map padding.
|
||||
4. Keep `EnableWebVisualization=true` and set `EnableNativePainterVisualization=false` unless the Painter audit output is
|
||||
explicitly required and reviewed for this vehicle session.
|
||||
## 常见错误（Common Errors）
|
||||
|
||||
To start, select the entry in the vehicle UI and start the MovementTest. Confirm the status begins with
|
||||
`OBSERVE_ONLY: no chassis command is sent.`, then review the bootstrap status and the
|
||||
`TrajectoryObserver.World`, `TrajectoryObserver.LS`, and `TrajectoryObserver.ST` layers. A logged trajectory control
|
||||
command is diagnostic information only and must not be copied into a vehicle-control interface.
|
||||
| 现象 | 检查项 |
|
||||
| --- | --- |
|
||||
| 启动即提示定位或底盘接口不可用 | 确认 MDCS 定位和底盘只读接口已连接；观察与闭环入口都需要初始状态。 |
|
||||
| 目标或障碍物输入被拒绝 | 检查所有值为有限数；障碍物数量为 `0–20`；半径/边长为正；包络位于地图范围内。 |
|
||||
| 设置校验失败 | 正数配置不得为零或非有限；`OutputTimeStepSeconds` 不得大于 `TimeHorizonSeconds`；端口只能为 `0` 或 `1024–65535`。 |
|
||||
| 观察测试没有车辆动作 | 这是预期行为。该入口永远是 `OBSERVE_ONLY`。 |
|
||||
| 闭环测试未接管车辆 | 检查首段规划是否成功、控制轨迹是否至少有两个不同位置点、底盘类型以及舵轮前向对齐。 |
|
||||
| 闭环在换向边界停止 | 这是预期行为；该测试不自动执行下一方向段。 |
|
||||
| 网页没有打开或无法访问 | 从控制台复制完整令牌 URL；确认使用本机回环地址；打开失败不会停止会话。 |
|
||||
|
||||
To stop, use the vehicle UI's normal MovementTest stop action. Confirm the status says
|
||||
`Observation stop requested; all observer layers were cleared.` The cancellation request stops observer work and clears
|
||||
an existing Painter, stops the web publisher/HTTP/SSE service, and releases its port. A current-session runtime fault
|
||||
also clears an existing Painter and stops the web session so stale diagnostics are not left on screen; an intentional
|
||||
bootstrap failure still reports through UI/console even when both visualization modes are off. Closing a browser tab does
|
||||
not stop the session; `TestStop()` owns final web and Painter cleanup. Stopping, running, or starting this test must not
|
||||
issue chassis, motor, steering, brake, or gear output.
|
||||
## 当前限制（Current Limits）
|
||||
|
||||
- 地图和障碍物只在启动时冻结；本模块不实现实时感知更新或全局重规划。
|
||||
- 观察测试不具备任何底盘、转向、制动或换向命令能力。
|
||||
- 闭环测试只覆盖冻结的首个方向段，换向后续段不自动执行。
|
||||
- 闭环控制依赖现有 `TrajectoryTrackingMovement` 和 `MultiWheelChassis`；它不是对任意底盘的通用执行接口。
|
||||
- 网页服务仅限本机回环访问，且可视化故障只作为诊断处理。
|
||||
|
||||
+2
@@ -7,6 +7,7 @@ using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>观察模式下的不可变诊断消息；错误只报告给操作者，不能触发车辆动作。</summary>
|
||||
public sealed class TrajectoryObservationDiagnostic
|
||||
{
|
||||
public TrajectoryObservationDiagnostic(string text)
|
||||
@@ -17,6 +18,7 @@ public sealed class TrajectoryObservationDiagnostic
|
||||
public string Text { get; }
|
||||
}
|
||||
|
||||
/// <summary>构造带会话上下文的统一观察诊断文本。</summary>
|
||||
public static class TrajectoryObservationDiagnostics
|
||||
{
|
||||
public static TrajectoryObservationDiagnostic CreateCurveReport(TrajectoryObservationCharts charts,
|
||||
|
||||
+1
@@ -7,6 +7,7 @@ using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>将每个 tick 的新鲜车辆状态与已发布轨迹转换为不可变动态可视化快照。</summary>
|
||||
public sealed class TrajectoryObservationDynamicSnapshotBuilder
|
||||
{
|
||||
private readonly TrajectoryObservationKinematicChartBuilder charts = new TrajectoryObservationKinematicChartBuilder();
|
||||
|
||||
+2
@@ -4,6 +4,7 @@ using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>描述相邻已发布轨迹在交接处的位置、航向和速度连续性，仅作诊断。</summary>
|
||||
public sealed class TrajectoryObservationHandoffMetrics
|
||||
{
|
||||
internal TrajectoryObservationHandoffMetrics(bool available, double? position, double? referenceS, double? velocity, double? acceleration)
|
||||
@@ -15,6 +16,7 @@ public sealed class TrajectoryObservationHandoffMetrics
|
||||
public double? DeltaAccelerationMetersPerSecondSquared { get; }
|
||||
}
|
||||
|
||||
/// <summary>计算观察会话中的轨迹交接指标，不参与轨迹发布或控制决策。</summary>
|
||||
public sealed class TrajectoryObservationHandoffAnalyzer
|
||||
{
|
||||
public TrajectoryObservationHandoffMetrics Analyze(EmTrajectory current, EmTrajectory previous, DirectionSegmentView segment)
|
||||
|
||||
+1
@@ -7,6 +7,7 @@ using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>建立轨迹的时间、路径弧长、速度、加速度和 jerk 图表数据；仅用于观察与诊断。</summary>
|
||||
public sealed class TrajectoryObservationKinematicChartBuilder
|
||||
{
|
||||
public IReadOnlyList<VisualizationChart> Build(EmTrajectory trajectory, DirectionSegmentView segment,
|
||||
|
||||
+150
-9
@@ -1,6 +1,7 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
@@ -12,6 +13,7 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>一次观察会话启动的不可变结果，成功时同时携带冻结的粗路径、平滑路径、地图和方向段。</summary>
|
||||
public sealed class TrajectoryObservationBootstrapResult
|
||||
{
|
||||
private readonly VehicleParameters vehicle;
|
||||
@@ -87,6 +89,7 @@ public sealed class TrajectoryObservationBootstrapResult
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>只在会话开始阶段构造并冻结规划输入；后续观察 tick 不会重新读取或改写该快照。</summary>
|
||||
public sealed class TrajectoryObservationBootstrapper
|
||||
{
|
||||
private readonly CoarsePathPlanningService coarseService;
|
||||
@@ -109,22 +112,110 @@ public sealed class TrajectoryObservationBootstrapper
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
|
||||
CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
|
||||
TimeSpan configuredLimit = job.Configuration == null
|
||||
? TimeSpan.FromSeconds(5d)
|
||||
: job.Configuration.SearchTimeout;
|
||||
using var deadline = new TrajectoryObservationPlanningDeadline(configuredLimit, cancellationToken);
|
||||
return Bootstrap(job, deadline, cancellationToken);
|
||||
}
|
||||
|
||||
public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
|
||||
TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
if (deadline == null) throw new ArgumentNullException(nameof(deadline));
|
||||
if (DeadlineExpired(deadline))
|
||||
return DeadlineFailure(job, null, null, deadline, "coarse");
|
||||
|
||||
CoarsePathPlanningJob effectiveJob = CopyJobWithDeadline(job, deadline);
|
||||
bool coarseUsesCycleRemainder = job.Configuration != null &&
|
||||
effectiveJob.Configuration.SearchTimeout < job.Configuration.SearchTimeout;
|
||||
using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
deadline.Token, cancellationToken);
|
||||
|
||||
CoarsePathPlanningJobResult coarse = coarseService.Plan(effectiveJob, linkedCancellation.Token);
|
||||
if (DeadlineExpired(deadline) ||
|
||||
(coarseUsesCycleRemainder && coarse.PlanningResult.Status == PlanningStatus.SearchTimeout))
|
||||
{
|
||||
return DeadlineFailure(job, coarse, null, deadline, "coarse");
|
||||
}
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
|
||||
|
||||
if (DeadlineExpired(deadline))
|
||||
return DeadlineFailure(job, coarse, null, deadline, "smoothing");
|
||||
|
||||
var smoothingRequest = new PathSmoothingRequest(
|
||||
CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
|
||||
coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
|
||||
new PathSmoothingConfiguration());
|
||||
PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, cancellationToken);
|
||||
PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, linkedCancellation.Token);
|
||||
if (DeadlineExpired(deadline))
|
||||
return DeadlineFailure(job, coarse, smooth, deadline, "smoothing");
|
||||
if (!IsPublishedSmoothingStatus(smooth.Status))
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, smooth, smooth.Diagnostics.TerminationReason);
|
||||
return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
|
||||
}
|
||||
|
||||
private static bool DeadlineExpired(TrajectoryObservationPlanningDeadline deadline)
|
||||
{
|
||||
return deadline.IsExpired && !deadline.CallerCancellationRequested;
|
||||
}
|
||||
|
||||
private static TrajectoryObservationBootstrapResult DeadlineFailure(CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarse, PathSmoothingResult smooth,
|
||||
TrajectoryObservationPlanningDeadline deadline, string phase)
|
||||
{
|
||||
string diagnostic = "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
|
||||
deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture);
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(job, coarse, smooth, diagnostic);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CopyJobWithDeadline(CoarsePathPlanningJob source,
|
||||
TrajectoryObservationPlanningDeadline deadline)
|
||||
{
|
||||
return new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = source.MapRequest,
|
||||
Start = source.Start,
|
||||
Goal = source.Goal,
|
||||
Vehicle = source.Vehicle,
|
||||
Configuration = CopyConfigurationWithDeadline(source.Configuration, deadline),
|
||||
StartVehicleCurvature = source.StartVehicleCurvature,
|
||||
StartDirection = source.StartDirection,
|
||||
GoalDirection = source.GoalDirection,
|
||||
DebugOptions = source.DebugOptions,
|
||||
};
|
||||
}
|
||||
|
||||
private static HybridAStarConfiguration CopyConfigurationWithDeadline(HybridAStarConfiguration source,
|
||||
TrajectoryObservationPlanningDeadline deadline)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new HybridAStarConfiguration
|
||||
{
|
||||
PrimitiveLengthMeters = source.PrimitiveLengthMeters,
|
||||
IntegrationStepMeters = source.IntegrationStepMeters,
|
||||
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
||||
HeadingResolutionRadians = source.HeadingResolutionRadians,
|
||||
CurvatureLevelCount = source.CurvatureLevelCount,
|
||||
GoalPositionToleranceMeters = source.GoalPositionToleranceMeters,
|
||||
GoalHeadingToleranceRadians = source.GoalHeadingToleranceRadians,
|
||||
MaximumExpandedNodes = source.MaximumExpandedNodes,
|
||||
SearchTimeout = deadline.Clamp(source.SearchTimeout),
|
||||
HeuristicWeight = source.HeuristicWeight,
|
||||
ReverseCostMultiplier = source.ReverseCostMultiplier,
|
||||
GearSwitchPenaltyMeters = source.GearSwitchPenaltyMeters,
|
||||
CurvatureMagnitudeWeight = source.CurvatureMagnitudeWeight,
|
||||
CurvatureChangePenaltyMetersPerLevel = source.CurvatureChangePenaltyMetersPerLevel,
|
||||
ClearanceCostWeight = source.ClearanceCostWeight,
|
||||
ClearanceCostDistanceMeters = source.ClearanceCostDistanceMeters,
|
||||
AllowReverse = source.AllowReverse,
|
||||
};
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
|
||||
PlanningGridMap map)
|
||||
{
|
||||
@@ -158,6 +249,7 @@ public sealed class TrajectoryObservationBootstrapper
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>单个观察 tick 的只读结果，供状态文本、报告和可视化共同消费。</summary>
|
||||
public sealed class TrajectoryObservationObservation
|
||||
{
|
||||
internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
|
||||
@@ -185,6 +277,7 @@ public sealed class TrajectoryObservationObservation
|
||||
public TrajectoryExecutionState ExecutorState { get; }
|
||||
}
|
||||
|
||||
/// <summary>协调一次冻结会话中的 EM 规划、轨迹采样和诊断；从不将预测命令发送给车辆。</summary>
|
||||
public sealed class TrajectoryObservationController
|
||||
{
|
||||
private readonly TrajectoryObservationBootstrapResult bootstrap;
|
||||
@@ -272,8 +365,19 @@ public sealed class TrajectoryObservationController
|
||||
|
||||
public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
using var deadline = new TrajectoryObservationPlanningDeadline(
|
||||
TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), cancellationToken);
|
||||
return await StartCycle(now, state, deadline, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
||||
TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken)
|
||||
{
|
||||
if (state == null) throw new ArgumentNullException(nameof(state));
|
||||
if (deadline == null) throw new ArgumentNullException(nameof(deadline));
|
||||
using var callerCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
deadline.CallerToken, cancellationToken);
|
||||
|
||||
int targetSegmentIndex = ActiveSegment.SegmentIndex;
|
||||
int pendingSegmentIndex = -1;
|
||||
@@ -303,18 +407,53 @@ public sealed class TrajectoryObservationController
|
||||
}
|
||||
|
||||
long currentCycleId = Interlocked.Increment(ref cycleId);
|
||||
EmPlannerConfiguration effectiveConfiguration = configuration.Copy();
|
||||
TimeSpan cycleRemaining = deadline.Remaining;
|
||||
TimeSpan configuredSolverLimit = TimeSpan.FromSeconds(
|
||||
configuration.Scheduling.SolverTimeoutSeconds);
|
||||
if (cycleRemaining > TimeSpan.Zero &&
|
||||
cycleRemaining < configuredSolverLimit - TimeSpan.FromMilliseconds(1d))
|
||||
{
|
||||
effectiveConfiguration.Scheduling.SolverTimeoutSeconds = cycleRemaining.TotalSeconds;
|
||||
}
|
||||
var request = new EmPlanningRequest(
|
||||
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
|
||||
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, effectiveConfiguration,
|
||||
targetSegmentIndex, previousTrajectory, now, now,
|
||||
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
|
||||
previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
|
||||
EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope);
|
||||
PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken)
|
||||
.ConfigureAwait(false);
|
||||
if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment && result.Published &&
|
||||
result.Result.Trajectory != null && !planningPendingFullDirection)
|
||||
EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope, cycleRemaining,
|
||||
callerCancellation.Token, deadline.DeadlineToken,
|
||||
deadline.PublicationAuthorization);
|
||||
if (callerCancellation.IsCancellationRequested)
|
||||
{
|
||||
activeFullDirectionTrajectory = result.Result.Trajectory;
|
||||
var cancelledResult = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
|
||||
"callerCancellation=true;phase=request");
|
||||
return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
|
||||
cancelledResult, false, cancelledResult.FailureReason);
|
||||
}
|
||||
if (deadline.DeadlineToken.IsCancellationRequested)
|
||||
{
|
||||
var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
||||
"cycleDeadlineExpired=true;phase=request;remainingMs=" +
|
||||
cycleRemaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
||||
return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
|
||||
expiredResult, false, expiredResult.FailureReason);
|
||||
}
|
||||
Action<EmTrajectory> fullDirectionCommit =
|
||||
settings.PlanningScope == EmPlanningScope.FullDirectionSegment && !planningPendingFullDirection
|
||||
? trajectory => activeFullDirectionTrajectory = trajectory
|
||||
: null;
|
||||
PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(
|
||||
new PlanningCycleInput(request, now), callerCancellation.Token,
|
||||
fullDirectionCommit).ConfigureAwait(false);
|
||||
if (!result.Published && deadline.IsExpired && !callerCancellation.IsCancellationRequested &&
|
||||
result.Result.Status == EmPlanningStatus.Cancelled)
|
||||
{
|
||||
var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
||||
"cycleDeadlineExpired=true;phase=planning;remainingMs=" +
|
||||
deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
||||
result = new PlanningCycleResult(result.Version, result.Identity, expiredResult, false,
|
||||
expiredResult.FailureReason);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
@@ -454,6 +593,7 @@ public sealed class TrajectoryObservationLoopTick
|
||||
public bool ShouldLog => true;
|
||||
}
|
||||
|
||||
/// <summary>按观察周期捕获新鲜只读状态并发布快照的循环；规划耗时不能改变观察安全边界。</summary>
|
||||
public sealed class TrajectoryObservationLoop
|
||||
{
|
||||
private readonly TrajectoryObservationController controller;
|
||||
@@ -511,6 +651,7 @@ public enum TrajectoryObservationSessionEndReason
|
||||
Cancellation,
|
||||
}
|
||||
|
||||
/// <summary>集中定义观察会话的开始、停止和故障清理语义，避免留下旧会话可视化。</summary>
|
||||
public static class TrajectoryObservationSessionLifecycle
|
||||
{
|
||||
public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
|
||||
|
||||
+226
@@ -0,0 +1,226 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>Owns one monotonic time budget shared by every phase of an observation planning cycle.</summary>
|
||||
public sealed class TrajectoryObservationPlanningDeadline : IDisposable
|
||||
{
|
||||
private static readonly TimeSpan MaximumTimerDelay = TimeSpan.FromMilliseconds(uint.MaxValue - 1d);
|
||||
private readonly object authorizationGate = new object();
|
||||
private readonly TimeSpan limit;
|
||||
private readonly Stopwatch stopwatch;
|
||||
private readonly TimeSpan? injectedElapsed;
|
||||
private readonly CancellationToken callerToken;
|
||||
private readonly CancellationTokenSource expirationCancellation;
|
||||
private readonly CancellationTokenSource linkedCancellation;
|
||||
private readonly Timer expirationTimer;
|
||||
private readonly IEmPlanningPublicationAuthorization publicationAuthorization;
|
||||
private bool callerCancellationObserved;
|
||||
private bool deadlineExpired;
|
||||
private bool disposed;
|
||||
|
||||
public TrajectoryObservationPlanningDeadline(TimeSpan limit, CancellationToken callerToken = default)
|
||||
: this(limit, null, callerToken)
|
||||
{
|
||||
}
|
||||
|
||||
private TrajectoryObservationPlanningDeadline(TimeSpan limit, TimeSpan? injectedElapsed,
|
||||
CancellationToken callerToken)
|
||||
{
|
||||
if (limit < TimeSpan.Zero)
|
||||
throw new ArgumentOutOfRangeException(nameof(limit), "The planning deadline cannot be negative.");
|
||||
if (injectedElapsed.HasValue && injectedElapsed.Value < TimeSpan.Zero)
|
||||
throw new ArgumentOutOfRangeException(nameof(injectedElapsed));
|
||||
|
||||
this.limit = limit;
|
||||
this.injectedElapsed = injectedElapsed;
|
||||
this.callerToken = callerToken;
|
||||
stopwatch = injectedElapsed.HasValue ? null : Stopwatch.StartNew();
|
||||
expirationCancellation = new CancellationTokenSource();
|
||||
TimeSpan initialRemaining = ComputeRemaining();
|
||||
deadlineExpired = initialRemaining <= TimeSpan.Zero;
|
||||
if (deadlineExpired)
|
||||
expirationCancellation.Cancel();
|
||||
linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
callerToken, expirationCancellation.Token);
|
||||
publicationAuthorization = new DeadlinePublicationAuthorization(this);
|
||||
if (!deadlineExpired)
|
||||
{
|
||||
TimeSpan timerDelay = initialRemaining < MaximumTimerDelay
|
||||
? initialRemaining
|
||||
: MaximumTimerDelay;
|
||||
expirationTimer = new Timer(ExpireFromTimer, null, timerDelay, Timeout.InfiniteTimeSpan);
|
||||
}
|
||||
}
|
||||
|
||||
public TimeSpan Remaining
|
||||
{
|
||||
get
|
||||
{
|
||||
bool cancelExpiration = false;
|
||||
TimeSpan remaining;
|
||||
lock (authorizationGate)
|
||||
{
|
||||
remaining = ComputeRemaining();
|
||||
if (!deadlineExpired && remaining <= TimeSpan.Zero)
|
||||
{
|
||||
deadlineExpired = true;
|
||||
cancelExpiration = true;
|
||||
}
|
||||
if (deadlineExpired)
|
||||
remaining = TimeSpan.Zero;
|
||||
}
|
||||
if (cancelExpiration)
|
||||
CancelExpirationToken();
|
||||
return remaining;
|
||||
}
|
||||
}
|
||||
|
||||
public bool IsExpired => callerToken.IsCancellationRequested || DeadlineExpired;
|
||||
|
||||
public CancellationToken Token => linkedCancellation.Token;
|
||||
|
||||
internal bool CallerCancellationRequested => callerToken.IsCancellationRequested;
|
||||
|
||||
internal CancellationToken CallerToken => callerToken;
|
||||
|
||||
internal CancellationToken DeadlineToken => expirationCancellation.Token;
|
||||
|
||||
internal IEmPlanningPublicationAuthorization PublicationAuthorization => publicationAuthorization;
|
||||
|
||||
internal static TrajectoryObservationPlanningDeadline CreateForTesting(TimeSpan limit,
|
||||
TimeSpan elapsed, CancellationToken callerToken)
|
||||
{
|
||||
return new TrajectoryObservationPlanningDeadline(limit, elapsed, callerToken);
|
||||
}
|
||||
|
||||
public TimeSpan Clamp(TimeSpan configuredLimit)
|
||||
{
|
||||
if (configuredLimit < TimeSpan.Zero)
|
||||
throw new ArgumentOutOfRangeException(nameof(configuredLimit));
|
||||
TimeSpan remaining = Remaining;
|
||||
return configuredLimit < remaining ? configuredLimit : remaining;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (authorizationGate)
|
||||
disposed = true;
|
||||
expirationTimer?.Dispose();
|
||||
linkedCancellation.Dispose();
|
||||
expirationCancellation.Dispose();
|
||||
}
|
||||
|
||||
private bool DeadlineExpired
|
||||
{
|
||||
get
|
||||
{
|
||||
_ = Remaining;
|
||||
lock (authorizationGate)
|
||||
return deadlineExpired;
|
||||
}
|
||||
}
|
||||
|
||||
private TimeSpan ComputeRemaining()
|
||||
{
|
||||
TimeSpan elapsed = injectedElapsed ?? stopwatch.Elapsed;
|
||||
TimeSpan remaining = limit - elapsed;
|
||||
return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
|
||||
}
|
||||
|
||||
private EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish)
|
||||
{
|
||||
if (publish == null)
|
||||
throw new ArgumentNullException(nameof(publish));
|
||||
|
||||
bool cancelExpiration = false;
|
||||
using CancellationTokenRegistration ownerCallerRegistration =
|
||||
callerToken.Register(ObserveCallerCancellation);
|
||||
using CancellationTokenRegistration requestCallerRegistration =
|
||||
requestCallerCancellationToken.Register(ObserveCallerCancellation);
|
||||
using CancellationTokenRegistration coordinatorCallerRegistration =
|
||||
coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
|
||||
EmPlanningPublicationDecision decision;
|
||||
lock (authorizationGate)
|
||||
{
|
||||
if (callerToken.IsCancellationRequested ||
|
||||
requestCallerCancellationToken.IsCancellationRequested ||
|
||||
coordinatorCallerCancellationToken.IsCancellationRequested)
|
||||
{
|
||||
callerCancellationObserved = true;
|
||||
}
|
||||
if (!deadlineExpired && ComputeRemaining() <= TimeSpan.Zero)
|
||||
{
|
||||
deadlineExpired = true;
|
||||
cancelExpiration = true;
|
||||
}
|
||||
if (callerCancellationObserved)
|
||||
decision = EmPlanningPublicationDecision.CallerCancelled;
|
||||
else if (deadlineExpired || disposed)
|
||||
decision = EmPlanningPublicationDecision.DeadlineExpired;
|
||||
else
|
||||
{
|
||||
publish();
|
||||
decision = EmPlanningPublicationDecision.Published;
|
||||
}
|
||||
}
|
||||
if (cancelExpiration)
|
||||
CancelExpirationToken();
|
||||
return decision;
|
||||
}
|
||||
|
||||
private void ObserveCallerCancellation()
|
||||
{
|
||||
lock (authorizationGate)
|
||||
callerCancellationObserved = true;
|
||||
}
|
||||
|
||||
private void ExpireFromTimer(object state)
|
||||
{
|
||||
bool cancelExpiration = false;
|
||||
lock (authorizationGate)
|
||||
{
|
||||
if (!disposed && !deadlineExpired)
|
||||
{
|
||||
deadlineExpired = true;
|
||||
cancelExpiration = true;
|
||||
}
|
||||
}
|
||||
if (cancelExpiration)
|
||||
CancelExpirationToken();
|
||||
}
|
||||
|
||||
private void CancelExpirationToken()
|
||||
{
|
||||
try
|
||||
{
|
||||
expirationCancellation.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// Disposal can race a timer callback only after the owning cycle has completed.
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class DeadlinePublicationAuthorization : IEmPlanningPublicationAuthorization
|
||||
{
|
||||
private readonly TrajectoryObservationPlanningDeadline owner;
|
||||
|
||||
public DeadlinePublicationAuthorization(TrajectoryObservationPlanningDeadline owner)
|
||||
{
|
||||
this.owner = owner;
|
||||
}
|
||||
|
||||
public TimeSpan Remaining => owner.Remaining;
|
||||
|
||||
public bool IsExpired => owner.DeadlineExpired;
|
||||
|
||||
public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish) =>
|
||||
owner.TryPublish(requestCallerCancellationToken, coordinatorCallerCancellationToken, publish);
|
||||
}
|
||||
}
|
||||
+2
@@ -13,6 +13,7 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>将只读观察结果转换为面向操作者的中文文本，不包含控制指令下发逻辑。</summary>
|
||||
public static class TrajectoryObservationPresentationText
|
||||
{
|
||||
public static string Create(TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
|
||||
@@ -77,6 +78,7 @@ public sealed class TrajectoryObservationLsPresentationModel
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>组合当前会话的文本、图层和图表展示模型。</summary>
|
||||
public sealed class TrajectoryObservationPresentation
|
||||
{
|
||||
private const float MillimetersPerMeter = 1000f;
|
||||
|
||||
+2
@@ -6,6 +6,7 @@ using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>观察器对当前方向段和真实换向确认过程的只读阶段。</summary>
|
||||
public enum TrajectoryObservationSegmentPhase
|
||||
{
|
||||
Planning,
|
||||
@@ -54,6 +55,7 @@ public sealed class TrajectoryObservationSegmentUpdate
|
||||
public TrajectoryObservationSegmentState State { get; }
|
||||
}
|
||||
|
||||
/// <summary>根据真实状态和方向确认推进活动段；不会自行请求或执行车辆换向。</summary>
|
||||
public sealed class TrajectoryObservationSegmentTracker
|
||||
{
|
||||
private readonly IReadOnlyList<DirectionSegmentView> segments;
|
||||
|
||||
+1
@@ -8,6 +8,7 @@ using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>从冻结的规划输入建立世界和参考路径静态快照，供会话期间重复只读使用。</summary>
|
||||
public sealed class TrajectoryObservationStaticSnapshotBuilder
|
||||
{
|
||||
public PlanningVisualizationStaticSnapshot Build(TrajectoryObservationBootstrapResult bootstrap,
|
||||
|
||||
Reference in New Issue
Block a user