630 lines
30 KiB
C#
630 lines
30 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.DTools;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using FundamentalLib;
|
||
using MDCSToolBox.Clumsy.Pilot;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||
using TrajectoryPlanningVisualization;
|
||
|
||
namespace MultiWheelC;
|
||
|
||
[MovementTest(name = "EM轨迹规划观察闭环测试")]
|
||
public sealed class TrajectoryObservationMovementTest : MovementTest
|
||
{
|
||
private const int MaximumManualObstacleCount = 20;
|
||
private static long nextObstacleSnapshotVersion;
|
||
|
||
public double GoalXmm = double.NaN;
|
||
public double GoalYmm = double.NaN;
|
||
public double GoalYawDeg = 0d;
|
||
public double MapPaddingMeters = 2d;
|
||
public float MapResolutionMm = 50f;
|
||
public double ReplanPeriodSeconds = 0.20d;
|
||
public double ObserverPeriodSeconds = 0.05d;
|
||
public bool UseFullDirectionSegmentPlanning = true;
|
||
public double SolverTimeoutSeconds = 5.0d;
|
||
public int MaximumOsqpIterations = 100000;
|
||
/// <summary>仅滚动兼容字段；完整方向段模式不用于截断。默认 2s，不等于观察循环周期。</summary>
|
||
public double TimeHorizonSeconds = 20d;
|
||
/// <summary>Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；默认 0.10s，不等于观察循环周期。</summary>
|
||
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 = 0;
|
||
public double WebRefreshRateHz = 10d;
|
||
public int VisualizationHistoryCycleLimit = 60;
|
||
public bool EnableNativePainterVisualization = true;
|
||
public double DirectionConfirmationSpeedMetersPerSecond = 0.02d;
|
||
public int DirectionConfirmationSamples = 3;
|
||
public double GearSwitchProjectionToleranceMeters = 0.50d;
|
||
public double GearSwitchStopHoldSeconds = 0.20d;
|
||
|
||
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));
|
||
|
||
var settings = new TrajectoryObservationSettings
|
||
{
|
||
MapPaddingMeters = MapPaddingMeters,
|
||
MapResolutionMillimeters = MapResolutionMm,
|
||
ReplanPeriodSeconds = ReplanPeriodSeconds,
|
||
ObserverPeriodSeconds = ObserverPeriodSeconds,
|
||
PlanningScope = UseFullDirectionSegmentPlanning ? EmPlanningScope.FullDirectionSegment : EmPlanningScope.RollingHorizon,
|
||
SolverTimeoutSeconds = SolverTimeoutSeconds,
|
||
MaximumOsqpIterations = MaximumOsqpIterations,
|
||
TimeHorizonSeconds = TimeHorizonSeconds,
|
||
OutputTimeStepSeconds = OutputTimeStepSeconds,
|
||
VehicleLengthMeters = VehicleLengthMeters,
|
||
VehicleWidthMeters = VehicleWidthMeters,
|
||
SafetyMarginMeters = SafetyMarginMeters,
|
||
MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
|
||
EnableWebVisualization = EnableWebVisualization,
|
||
AutoOpenWebVisualization = AutoOpenWebVisualization,
|
||
WebVisualizationPort = WebVisualizationPort,
|
||
WebRefreshRateHz = WebRefreshRateHz,
|
||
VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
|
||
EnableNativePainterVisualization = EnableNativePainterVisualization,
|
||
DirectionConfirmationSpeedMetersPerSecond = DirectionConfirmationSpeedMetersPerSecond,
|
||
DirectionConfirmationSamples = DirectionConfirmationSamples,
|
||
GearSwitchProjectionToleranceMeters = GearSwitchProjectionToleranceMeters,
|
||
GearSwitchStopHoldSeconds = GearSwitchStopHoldSeconds,
|
||
};
|
||
settings = settings.CreateValidatedSnapshot();
|
||
TimeSpan observerPeriod = TimeSpan.FromSeconds(settings.ObserverPeriodSeconds);
|
||
IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
|
||
long obstacleSnapshotVersion = obstacles.Count == 0
|
||
? 0L
|
||
: Interlocked.Increment(ref nextObstacleSnapshotVersion);
|
||
var goal = new Pose2D(goalXMillimeters / 1000d, goalYMillimeters / 1000d,
|
||
goalYawDegrees * Math.PI / 180d);
|
||
|
||
TrajectoryObservationMovementTestRunner.Start(
|
||
goal, settings, observerPeriod, obstacles, obstacleSnapshotVersion);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
TrajectoryObservationMovementTestRunner.ShowInputFailure(exception);
|
||
}
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
TrajectoryObservationMovementTestRunner.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);
|
||
if (value <= 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须是正数：" + 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 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);
|
||
}
|
||
}
|
||
|
||
internal static class TrajectoryObservationMovementTestRunner
|
||
{
|
||
private const string StatusChannel = "TrajectoryObserver";
|
||
private const string ObserveOnlyNotice = "OBSERVE_ONLY: no chassis command is sent.";
|
||
private static readonly object SessionSync = new object();
|
||
|
||
private static CancellationTokenSource activeCancellation;
|
||
private static Task activeTask;
|
||
private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
|
||
private static TrajectoryObservationPresentation activePresentation;
|
||
private static TrajectoryObservationReportWriter activeReportWriter;
|
||
private static long nextSessionId;
|
||
private static long activeSessionId;
|
||
private static long stateSequence;
|
||
|
||
internal static void Start(Pose2D goal, TrajectoryObservationSettings settings, TimeSpan observerPeriod,
|
||
IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion)
|
||
{
|
||
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;
|
||
long sessionId;
|
||
TrajectoryObservationReportWriter reportWriter = null;
|
||
lock (SessionSync)
|
||
{
|
||
previousCancellation = activeCancellation;
|
||
activeCancellation = cancellation;
|
||
activeTask = null;
|
||
sessionId = ++nextSessionId;
|
||
activeSessionId = sessionId;
|
||
StopActiveVisualizationNoLock();
|
||
activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null;
|
||
reportWriter = new TrajectoryObservationReportWriter(sessionId);
|
||
activeReportWriter = reportWriter;
|
||
PrintStatus("Starting frozen observation session " +
|
||
sessionId.ToString(CultureInfo.InvariantCulture) + ".");
|
||
}
|
||
|
||
CancelWithoutWaiting(previousCancellation);
|
||
|
||
Task task = Task.Run(() => RunSessionAsync(sessionId, goal, settings, observerPeriod,
|
||
obstacles, obstacleSnapshotVersion, cancellation.Token), cancellation.Token);
|
||
lock (SessionSync)
|
||
{
|
||
if (activeSessionId == sessionId) activeTask = task;
|
||
}
|
||
_ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed, reportWriter), TaskScheduler.Default);
|
||
}
|
||
|
||
internal static void Stop()
|
||
{
|
||
CancellationTokenSource cancellation;
|
||
lock (SessionSync)
|
||
{
|
||
cancellation = activeCancellation;
|
||
activeCancellation = null;
|
||
activeTask = null;
|
||
activeSessionId = 0L;
|
||
StopActiveVisualizationNoLock();
|
||
PrintStatus("Observation stop requested; all observer layers were cleared.");
|
||
}
|
||
|
||
CancelWithoutWaiting(cancellation);
|
||
}
|
||
|
||
internal static void ShowInputFailure(Exception exception)
|
||
{
|
||
string message = exception == null ? "Unknown input error." : exception.Message;
|
||
PrintStatus("Observation session did not start: " + message);
|
||
}
|
||
|
||
private static async Task RunSessionAsync(long sessionId, Pose2D goal,
|
||
TrajectoryObservationSettings settings, TimeSpan observerPeriod,
|
||
IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion,
|
||
CancellationToken token)
|
||
{
|
||
token.ThrowIfCancellationRequested();
|
||
VehicleMotionState initialState = ReadVehicleState();
|
||
LogIfCurrent(sessionId, "initial state: pose=(" + Format(initialState.Pose.X) + ", " +
|
||
Format(initialState.Pose.Y) + ", " + Format(initialState.Pose.Heading) + ") m/rad, signed speed=" +
|
||
initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) + " m/s, acceleration=" +
|
||
(initialState.LongitudinalAccelerationMetersPerSecondSquared.HasValue
|
||
? Format(initialState.LongitudinalAccelerationMetersPerSecondSquared.Value)
|
||
: "unavailable") + " m/s2, stateSequence=" +
|
||
initialState.SequenceId.ToString(CultureInfo.InvariantCulture));
|
||
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||
initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
|
||
|
||
var bootstrapTimer = Stopwatch.StartNew();
|
||
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
||
.Bootstrap(job, token);
|
||
bootstrapTimer.Stop();
|
||
if (!bootstrap.Succeeded)
|
||
{
|
||
DrawIfCurrent(sessionId, bootstrap, null, null, null,
|
||
new TrajectoryObservationDiagnostic("bootstrap failed: " + bootstrap.FailureReason));
|
||
LogIfCurrent(sessionId, "Planning bootstrap failed after " +
|
||
bootstrapTimer.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
|
||
" ms: " + bootstrap.FailureReason);
|
||
return;
|
||
}
|
||
|
||
LogIfCurrent(sessionId, CreateBootstrapStatus(sessionId, bootstrap, settings, obstacles.Count,
|
||
bootstrapTimer.Elapsed));
|
||
var controller = new TrajectoryObservationController(bootstrap, settings,
|
||
new EmPlanningService(new OsqpNativeSolver()),
|
||
"trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
|
||
LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
|
||
LogIfCurrent(sessionId, CreateSettingsDiagnostic(settings));
|
||
EmPlannerConfiguration effectiveConfiguration = controller.CreateEffectiveConfigurationSnapshot();
|
||
double stopSpeedTolerance = effectiveConfiguration.Longitudinal.StopSpeedToleranceMetersPerSecond;
|
||
LogIfCurrent(sessionId, "static start check: initialSignedSpeed=" +
|
||
initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) +
|
||
", stopSpeedTolerance=" + stopSpeedTolerance.ToString("R", CultureInfo.InvariantCulture) +
|
||
", treatedAsStopped=" +
|
||
(Math.Abs(initialState.SignedLongitudinalSpeedMetersPerSecond) <= stopSpeedTolerance)
|
||
.ToString(CultureInfo.InvariantCulture));
|
||
var observationLoop = new TrajectoryObservationLoop(controller);
|
||
TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled(
|
||
sessionId, bootstrap, controller, settings, obstacleSnapshotVersion);
|
||
var dynamicSnapshotBuilder = new TrajectoryObservationDynamicSnapshotBuilder();
|
||
long visualizationSequence = 0L;
|
||
int lastLoggedWaitingSegmentIndex = -1;
|
||
|
||
while (true)
|
||
{
|
||
await Task.Delay(observerPeriod, token).ConfigureAwait(false);
|
||
VehicleMotionState state = ReadVehicleState();
|
||
DateTimeOffset now = state.CapturedAtUtc;
|
||
TrajectoryObservationLoopTick tick = observationLoop.Tick(now, state, token);
|
||
TrajectoryObservationObservation observation = tick.Observation;
|
||
TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create(
|
||
tick.LatestCycle, tick.LatestPlanningElapsed, tick.PlanningInFlight,
|
||
observation.PublishedTrajectory);
|
||
DirectionSegmentView segment = controller.ActiveSegment;
|
||
TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
|
||
? null
|
||
: TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
|
||
effectiveConfiguration.Frenet.MaximumProjectionDistanceMeters);
|
||
TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(controller.SegmentState);
|
||
DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
|
||
webPublisher?.TryPublish(now, () => dynamicSnapshotBuilder.Build(
|
||
Interlocked.Increment(ref visualizationSequence), tick, controller.ActiveSegment,
|
||
controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
|
||
controller.CreateEffectiveConfigurationSnapshot()));
|
||
if (runtimeState.WaitingAtGearSwitch && segment.SegmentIndex != lastLoggedWaitingSegmentIndex)
|
||
{
|
||
LogIfCurrent(sessionId, runtimeState.WorldNotice);
|
||
lastLoggedWaitingSegmentIndex = segment.SegmentIndex;
|
||
}
|
||
|
||
if (tick.PlanningStarted)
|
||
LogIfCurrent(sessionId, "planning status=pending");
|
||
if (tick.PlanningCompleted)
|
||
{
|
||
LogIfCurrent(sessionId, diagnostic.Text);
|
||
if (observation.PublishedTrajectory != null && charts != null)
|
||
{
|
||
AppendReportIfCurrent(sessionId,
|
||
TrajectoryObservationDiagnostics.CreateCurveReport(charts, observation.PublishedTrajectory).Text);
|
||
}
|
||
}
|
||
if (tick.SegmentState.Phase == TrajectoryObservationSegmentPhase.Completed)
|
||
{
|
||
LogIfCurrent(sessionId, "all direction segments completed; observation session ended.");
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
private static VehicleMotionState ReadVehicleState()
|
||
{
|
||
var location = DetourInterface.getCartLocation();
|
||
if (location == null) throw new InvalidOperationException("Live localization is unavailable.");
|
||
if (BasicPilotBase.Chassis == null) throw new InvalidOperationException("Live chassis read interface is unavailable.");
|
||
var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
|
||
return new VehicleMotionState(
|
||
new Pose2D(location.x / 1000d, location.y / 1000d, location.th * Math.PI / 180d),
|
||
speed.Vx, null, DateTimeOffset.UtcNow, Interlocked.Increment(ref stateSequence));
|
||
}
|
||
|
||
private static string CreateBootstrapStatus(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
|
||
TrajectoryObservationSettings settings, int obstacleCount, TimeSpan elapsed)
|
||
{
|
||
float widthMillimeters = bootstrap.Map.Bounds.XMax - bootstrap.Map.Bounds.XMin;
|
||
float heightMillimeters = bootstrap.Map.Bounds.YMax - bootstrap.Map.Bounds.YMin;
|
||
return "Session " + sessionId.ToString(CultureInfo.InvariantCulture) + " bootstrap succeeded in " +
|
||
elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms.\n" +
|
||
"start=(" + Format(bootstrap.Job.Start.X) + ", " + Format(bootstrap.Job.Start.Y) + ", " +
|
||
Format(bootstrap.Job.Start.Heading) + ") m/rad; goal=(" + Format(bootstrap.Job.Goal.X) + ", " +
|
||
Format(bootstrap.Job.Goal.Y) + ", " + Format(bootstrap.Job.Goal.Heading) + ") m/rad.\n" +
|
||
"map=" + Format(widthMillimeters) + " x " + Format(heightMillimeters) + " mm, resolution=" +
|
||
Format(settings.MapResolutionMillimeters) + " mm, padding=" + Format(settings.MapPaddingMeters) +
|
||
" m, obstacles=" + obstacleCount.ToString(CultureInfo.InvariantCulture) + ".\n" +
|
||
"replan=" + Format(settings.ReplanPeriodSeconds) + " s, observer=" +
|
||
Format(settings.ObserverPeriodSeconds) + " s, segment index=0.";
|
||
}
|
||
|
||
private static string CreateTickStatus(long sessionId, TrajectoryObservationObservation observation,
|
||
TrajectoryObservationCharts charts, PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed,
|
||
bool waitingAtGearSwitch)
|
||
{
|
||
string planning = latestCycle == null
|
||
? "not started"
|
||
: latestCycle.Result.Status + ", published=" + latestCycle.Published +
|
||
", version=" + latestCycle.Version.ToString(CultureInfo.InvariantCulture);
|
||
string selected = observation.SelectedPoint == null
|
||
? "selected trajectory point unavailable"
|
||
: "selected t=" + Format(observation.SelectedPoint.TimeFromStart) +
|
||
" s, path-S=" + Format(observation.SelectedPoint.PathS) + " m";
|
||
string loggedCommand = observation.Command == null
|
||
? "logged command unavailable"
|
||
: "logged command only: signed speed=" + Format(observation.Command.SignedLongitudinalVelocity) +
|
||
" m/s, yaw rate=" + Format(observation.Command.YawRate) + " rad/s, direction=" +
|
||
observation.Command.Direction + ", request direction change=" +
|
||
observation.Command.RequestDirectionChange;
|
||
string terminal = observation.PublishedTrajectory == null
|
||
? "trajectory unavailable"
|
||
: "trajectory=" + observation.PublishedTrajectory.Metadata.TrajectoryId + ", terminal=" +
|
||
observation.PublishedTrajectory.Metadata.TerminalType + ", segment index=" +
|
||
observation.PublishedTrajectory.Metadata.SegmentIndex.ToString(CultureInfo.InvariantCulture);
|
||
string projectionFailures = charts == null
|
||
? "unavailable"
|
||
: charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
|
||
string waiting = waitingAtGearSwitch
|
||
? "\n" + TrajectoryObservationRuntimeState.GearSwitchWaitingNotice
|
||
: string.Empty;
|
||
|
||
return "Session " + sessionId.ToString(CultureInfo.InvariantCulture) + ".\n" +
|
||
"pose=(" + Format(observation.VehicleState.Pose.X) + ", " +
|
||
Format(observation.VehicleState.Pose.Y) + ", " + Format(observation.VehicleState.Pose.Heading) +
|
||
") m/rad, actual longitudinal speed=" +
|
||
Format(observation.VehicleState.SignedLongitudinalSpeedMetersPerSecond) + " m/s.\n" +
|
||
"planning=" + planning + ", latest replan elapsed=" +
|
||
latestPlanningElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms.\n" +
|
||
terminal + ".\n" + selected + ".\n" + loggedCommand + ".\n" +
|
||
"LS projection failures=" + projectionFailures + "." + waiting;
|
||
}
|
||
|
||
private static TrajectoryObservationVisualizationPublisher StartWebVisualizationIfEnabled(long sessionId,
|
||
TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationController controller,
|
||
TrajectoryObservationSettings settings, long obstacleSnapshotVersion)
|
||
{
|
||
if (!settings.EnableWebVisualization) return null;
|
||
|
||
var publisher = new TrajectoryObservationVisualizationPublisher(settings,
|
||
new PlanningVisualizationSessionSink(), message => LogIfCurrent(sessionId, message));
|
||
PlanningVisualizationSessionInfo sessionInfo;
|
||
try
|
||
{
|
||
var staticSnapshot = new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
|
||
controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion);
|
||
sessionInfo = publisher.Start(staticSnapshot);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
publisher.Disable(exception);
|
||
return null;
|
||
}
|
||
|
||
if (sessionInfo == null)
|
||
return null;
|
||
if (!TryAttachWebPublisher(sessionId, publisher))
|
||
{
|
||
publisher.Stop();
|
||
return null;
|
||
}
|
||
|
||
LogIfCurrent(sessionId, "网页可视化地址（含会话令牌）：" + sessionInfo.Uri.AbsoluteUri);
|
||
if (settings.AutoOpenWebVisualization)
|
||
{
|
||
try
|
||
{
|
||
Process.Start(new ProcessStartInfo
|
||
{
|
||
FileName = sessionInfo.Uri.AbsoluteUri,
|
||
UseShellExecute = true,
|
||
});
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
LogIfCurrent(sessionId, "无法自动打开网页；服务继续运行，请手动访问：" +
|
||
sessionInfo.Uri.AbsoluteUri + "；" + exception.GetType().Name + "：" + exception.Message);
|
||
}
|
||
}
|
||
return publisher;
|
||
}
|
||
|
||
private static bool TryAttachWebPublisher(long sessionId,
|
||
TrajectoryObservationVisualizationPublisher publisher)
|
||
{
|
||
lock (SessionSync)
|
||
{
|
||
if (activeSessionId != sessionId) return false;
|
||
activeWebPublisher = publisher;
|
||
return true;
|
||
}
|
||
}
|
||
|
||
private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
|
||
TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
|
||
TrajectoryObservationRuntimeState runtimeState, TrajectoryObservationDiagnostic diagnostic)
|
||
{
|
||
lock (SessionSync)
|
||
{
|
||
if (activeSessionId != sessionId) return;
|
||
TrajectoryObservationPresentation presentation = activePresentation;
|
||
if (presentation == null) return;
|
||
string diagnosticText = diagnostic == null ? string.Empty : diagnostic.Text;
|
||
presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText);
|
||
presentation.DrawLs(charts, diagnosticText);
|
||
presentation.DrawSt(charts, diagnosticText);
|
||
}
|
||
}
|
||
|
||
private static void LogIfCurrent(long sessionId, string message)
|
||
{
|
||
lock (SessionSync)
|
||
{
|
||
if (activeSessionId != sessionId) return;
|
||
PrintStatus(message);
|
||
}
|
||
}
|
||
|
||
private static void AppendReportIfCurrent(long sessionId, string text)
|
||
{
|
||
lock (SessionSync)
|
||
{
|
||
if (activeSessionId != sessionId) return;
|
||
activeReportWriter?.Append(text);
|
||
}
|
||
}
|
||
|
||
private static void PrintStatus(string message)
|
||
{
|
||
string text = ObserveOnlyNotice + "\n" + message;
|
||
Hedingben.ToastText(text, StatusChannel);
|
||
Console.WriteLine("[TrajectoryObserver] " + text);
|
||
activeReportWriter?.Append(message);
|
||
}
|
||
|
||
private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed,
|
||
TrajectoryObservationReportWriter reportWriter)
|
||
{
|
||
try
|
||
{
|
||
completed.GetAwaiter().GetResult();
|
||
}
|
||
catch (OperationCanceledException)
|
||
{
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
ClearAndLogRuntimeFaultIfCurrent(sessionId, exception);
|
||
}
|
||
finally
|
||
{
|
||
lock (SessionSync)
|
||
{
|
||
if (ReferenceEquals(activeReportWriter, reportWriter))
|
||
activeReportWriter = null;
|
||
if (activeSessionId == sessionId)
|
||
{
|
||
activeTask = null;
|
||
activeCancellation = null;
|
||
}
|
||
}
|
||
try
|
||
{
|
||
string reportPath = reportWriter.Save();
|
||
Console.WriteLine("[TrajectoryObserver] report=" + reportPath);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
Console.WriteLine("[TrajectoryObserver] report write failed: " + exception.Message);
|
||
}
|
||
cancellation.Dispose();
|
||
}
|
||
}
|
||
|
||
private static string CreateSettingsDiagnostic(TrajectoryObservationSettings settings)
|
||
{
|
||
return "observation settings: planningScope=" + settings.PlanningScope +
|
||
", webVisualization=" + settings.EnableWebVisualization +
|
||
", nativePainter=" + settings.EnableNativePainterVisualization +
|
||
", outputTimeStep=" + Format(settings.OutputTimeStepSeconds) + "s" +
|
||
", observerPeriod=" + Format(settings.ObserverPeriodSeconds) + "s" +
|
||
", solverTimeout=" + Format(settings.SolverTimeoutSeconds) + "s" +
|
||
", maximumOsqpIterations=" + settings.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture);
|
||
}
|
||
|
||
private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
|
||
{
|
||
lock (SessionSync)
|
||
{
|
||
if (activeSessionId != sessionId) return;
|
||
StopActiveVisualizationNoLock();
|
||
PrintStatus("Observation session failed; all observer layers were cleared: " +
|
||
exception.GetType().Name + ": " + exception.Message);
|
||
}
|
||
}
|
||
|
||
private static void StopActiveVisualizationNoLock()
|
||
{
|
||
TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher;
|
||
activeWebPublisher = null;
|
||
publisher?.Stop();
|
||
|
||
TrajectoryObservationPresentation presentation = activePresentation;
|
||
activePresentation = null;
|
||
presentation?.ClearAll();
|
||
}
|
||
|
||
private static void CancelWithoutWaiting(CancellationTokenSource cancellation)
|
||
{
|
||
if (cancellation == null) return;
|
||
try
|
||
{
|
||
cancellation.Cancel();
|
||
}
|
||
catch (ObjectDisposedException)
|
||
{
|
||
}
|
||
}
|
||
|
||
private static string Format(double value)
|
||
{
|
||
return value.ToString("F3", CultureInfo.InvariantCulture);
|
||
}
|
||
}
|