443 lines
20 KiB
C#
443 lines
20 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;
|
|||
|
|
|
|||
|
|
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 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,
|
|||
|
|
};
|
|||
|
|
settings.Validate();
|
|||
|
|
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 const string GearSwitchWaitingNotice =
|
|||
|
|
"绛夊緟鐪熷疄妗ｄ綅/鏂瑰悜纭锛涜瀵熸ā寮忎笉浼氭帹杩涗笅涓€鏂瑰悜娈?";
|
|||
|
|
private static readonly TimeSpan StatusPeriod = TimeSpan.FromSeconds(1d);
|
|||
|
|
private static readonly object SessionSync = new object();
|
|||
|
|
private static readonly TrajectoryObservationPresentation Presentation =
|
|||
|
|
new TrajectoryObservationPresentation();
|
|||
|
|
|
|||
|
|
private static CancellationTokenSource activeCancellation;
|
|||
|
|
private static Task activeTask;
|
|||
|
|
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;
|
|||
|
|
lock (SessionSync)
|
|||
|
|
{
|
|||
|
|
previousCancellation = activeCancellation;
|
|||
|
|
activeCancellation = cancellation;
|
|||
|
|
activeTask = null;
|
|||
|
|
sessionId = ++nextSessionId;
|
|||
|
|
activeSessionId = sessionId;
|
|||
|
|
Presentation.ClearAll();
|
|||
|
|
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), TaskScheduler.Default);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
internal static void Stop()
|
|||
|
|
{
|
|||
|
|
CancellationTokenSource cancellation;
|
|||
|
|
lock (SessionSync)
|
|||
|
|
{
|
|||
|
|
cancellation = activeCancellation;
|
|||
|
|
activeCancellation = null;
|
|||
|
|
activeTask = null;
|
|||
|
|
activeSessionId = 0L;
|
|||
|
|
Presentation.ClearAll();
|
|||
|
|
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();
|
|||
|
|
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);
|
|||
|
|
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));
|
|||
|
|
DirectionSegmentView segment = bootstrap.Segments[0];
|
|||
|
|
DateTimeOffset nextStatusAt = DateTimeOffset.MinValue;
|
|||
|
|
PlanningCycleResult latestCycle = null;
|
|||
|
|
TimeSpan latestPlanningElapsed = TimeSpan.Zero;
|
|||
|
|
bool gearSwitchWaitingPrinted = false;
|
|||
|
|
|
|||
|
|
while (true)
|
|||
|
|
{
|
|||
|
|
await Task.Delay(observerPeriod, token).ConfigureAwait(false);
|
|||
|
|
DateTimeOffset now = DateTimeOffset.UtcNow;
|
|||
|
|
VehicleMotionState state = ReadVehicleState();
|
|||
|
|
if (controller.ShouldStartCycle(now))
|
|||
|
|
{
|
|||
|
|
var planningTimer = Stopwatch.StartNew();
|
|||
|
|
latestCycle = await controller.StartCycle(now, state, token).ConfigureAwait(false);
|
|||
|
|
planningTimer.Stop();
|
|||
|
|
latestPlanningElapsed = planningTimer.Elapsed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
TrajectoryObservationObservation observation = controller.Observe(now, state);
|
|||
|
|
TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
|
|||
|
|
? null
|
|||
|
|
: TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
|
|||
|
|
settings.MapPaddingMeters);
|
|||
|
|
DrawIfCurrent(sessionId, bootstrap, observation, charts);
|
|||
|
|
bool waitingAtGearSwitch = HasReachedGearSwitchFinal(now, observation.PublishedTrajectory);
|
|||
|
|
if (waitingAtGearSwitch && !gearSwitchWaitingPrinted)
|
|||
|
|
{
|
|||
|
|
LogIfCurrent(sessionId, GearSwitchWaitingNotice);
|
|||
|
|
gearSwitchWaitingPrinted = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (now >= nextStatusAt)
|
|||
|
|
{
|
|||
|
|
LogIfCurrent(sessionId, CreateTickStatus(sessionId, observation, charts,
|
|||
|
|
latestCycle, latestPlanningElapsed, waitingAtGearSwitch));
|
|||
|
|
nextStatusAt = now + StatusPeriod;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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 bool HasReachedGearSwitchFinal(DateTimeOffset now, EmTrajectory trajectory)
|
|||
|
|
{
|
|||
|
|
if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
|
|||
|
|
return false;
|
|||
|
|
EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
|
|||
|
|
return (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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" + 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 void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
|
|||
|
|
TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
|
|||
|
|
{
|
|||
|
|
lock (SessionSync)
|
|||
|
|
{
|
|||
|
|
if (activeSessionId != sessionId) return;
|
|||
|
|
Presentation.DrawWorld(bootstrap, observation);
|
|||
|
|
Presentation.DrawLs(charts);
|
|||
|
|
Presentation.DrawSt(charts);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static void LogIfCurrent(long sessionId, string message)
|
|||
|
|
{
|
|||
|
|
lock (SessionSync)
|
|||
|
|
{
|
|||
|
|
if (activeSessionId != sessionId) return;
|
|||
|
|
PrintStatus(message);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static void PrintStatus(string message)
|
|||
|
|
{
|
|||
|
|
Hedingben.ToastText(ObserveOnlyNotice + "\n" + message, StatusChannel);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
completed.GetAwaiter().GetResult();
|
|||
|
|
}
|
|||
|
|
catch (OperationCanceledException)
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
catch (Exception exception)
|
|||
|
|
{
|
|||
|
|
LogIfCurrent(sessionId, "Observation session failed: " +
|
|||
|
|
exception.GetType().Name + ": " + exception.Message);
|
|||
|
|
}
|
|||
|
|
finally
|
|||
|
|
{
|
|||
|
|
lock (SessionSync)
|
|||
|
|
{
|
|||
|
|
if (activeSessionId == sessionId)
|
|||
|
|
{
|
|||
|
|
activeTask = null;
|
|||
|
|
activeCancellation = null;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
cancellation.Dispose();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
}
|