698 lines
29 KiB
C#
698 lines
29 KiB
C#
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(
|
||
"四个舵轮尚未与车头方向一致，控制器未接管底盘。");
|
||
}
|
||
}
|
||
}
|