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);
|
||
}
|
||
}
|