feat: add EM observation planning pipeline
This commit is contained in:
梁薄云
+335
@@ -0,0 +1,335 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
public sealed class TrajectoryObservationBootstrapResult
|
||||
{
|
||||
private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
|
||||
PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
|
||||
{
|
||||
Job = job ?? throw new ArgumentNullException(nameof(job));
|
||||
CoarseResult = coarse;
|
||||
SmoothedPath = smoothedPath;
|
||||
Segments = CopySegments(segments);
|
||||
FailureReason = failureReason ?? string.Empty;
|
||||
Succeeded = coarse != null && smoothedPath != null && Segments.Count > 0 && FailureReason.Length == 0;
|
||||
}
|
||||
|
||||
public bool Succeeded { get; }
|
||||
|
||||
public CoarsePathPlanningJob Job { get; }
|
||||
|
||||
public CoarsePathPlanningJobResult CoarseResult { get; }
|
||||
|
||||
public PathSmoothingResult SmoothedPath { get; }
|
||||
|
||||
public IReadOnlyList<DirectionSegmentView> Segments { get; }
|
||||
|
||||
public string FailureReason { get; }
|
||||
|
||||
public PlanningGridMap Map => CoarseResult?.MapResult?.Map;
|
||||
|
||||
public static TrajectoryObservationBootstrapResult FromFailure(CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, string failureReason)
|
||||
{
|
||||
return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath,
|
||||
Array.Empty<DirectionSegmentView>(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason);
|
||||
}
|
||||
|
||||
public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath,
|
||||
IReadOnlyList<DirectionSegmentView> segments)
|
||||
{
|
||||
if (coarse == null) throw new ArgumentNullException(nameof(coarse));
|
||||
if (smoothedPath == null) throw new ArgumentNullException(nameof(smoothedPath));
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A successful bootstrap requires reference segments.", nameof(segments));
|
||||
return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath, segments, string.Empty);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<DirectionSegmentView> CopySegments(IReadOnlyList<DirectionSegmentView> source)
|
||||
{
|
||||
var copy = new List<DirectionSegmentView>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<DirectionSegmentView>(copy);
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationBootstrapper
|
||||
{
|
||||
private readonly CoarsePathPlanningService coarseService;
|
||||
private readonly PathSmoothingService smoothingService;
|
||||
|
||||
public TrajectoryObservationBootstrapper()
|
||||
: this(new CoarsePathPlanningService(), new PathSmoothingService())
|
||||
{
|
||||
}
|
||||
|
||||
public TrajectoryObservationBootstrapper(CoarsePathPlanningService coarseService,
|
||||
PathSmoothingService smoothingService)
|
||||
{
|
||||
this.coarseService = coarseService ?? throw new ArgumentNullException(nameof(coarseService));
|
||||
this.smoothingService = smoothingService ?? throw new ArgumentNullException(nameof(smoothingService));
|
||||
}
|
||||
|
||||
public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
|
||||
CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
|
||||
|
||||
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);
|
||||
if (!IsPublishedSmoothingStatus(smooth.Status))
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, smooth, smooth.Diagnostics.TerminationReason);
|
||||
return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
|
||||
PlanningGridMap map)
|
||||
{
|
||||
if (path == null) throw new ArgumentNullException(nameof(path));
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
|
||||
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
||||
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
||||
double mapDiagonalMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
||||
var copy = new List<CoarsePathPoint>(path.Count);
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = path[index] ?? throw new ArgumentException(
|
||||
"The coarse path cannot contain null points.", nameof(path));
|
||||
double clearance = double.IsPositiveInfinity(point.BodyClearance)
|
||||
? mapDiagonalMeters
|
||||
: point.BodyClearance;
|
||||
copy.Add(new CoarsePathPoint(point.X, point.Y, point.Heading, point.UnwrappedHeading,
|
||||
point.ArcLength, point.Direction, point.VehicleCurvature, clearance,
|
||||
point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(copy);
|
||||
}
|
||||
|
||||
private static bool IsPublishedSmoothingStatus(PathSmoothingStatus status)
|
||||
{
|
||||
return status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged;
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationObservation
|
||||
{
|
||||
internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
|
||||
EmTrajectory publishedTrajectory, EmTrajectoryPoint selectedPoint, TrajectoryControlCommand command,
|
||||
TrajectoryExecutionState executorState)
|
||||
{
|
||||
ObservedAtUtc = observedAtUtc;
|
||||
VehicleState = vehicleState ?? throw new ArgumentNullException(nameof(vehicleState));
|
||||
PublishedTrajectory = publishedTrajectory;
|
||||
SelectedPoint = selectedPoint;
|
||||
Command = command;
|
||||
ExecutorState = executorState;
|
||||
}
|
||||
|
||||
public DateTimeOffset ObservedAtUtc { get; }
|
||||
|
||||
public VehicleMotionState VehicleState { get; }
|
||||
|
||||
public EmTrajectory PublishedTrajectory { get; }
|
||||
|
||||
public EmTrajectoryPoint SelectedPoint { get; }
|
||||
|
||||
public TrajectoryControlCommand Command { get; }
|
||||
|
||||
public TrajectoryExecutionState ExecutorState { get; }
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationController
|
||||
{
|
||||
private readonly TrajectoryObservationBootstrapResult bootstrap;
|
||||
private readonly EmPlannerConfiguration configuration;
|
||||
private readonly EmPlanningCoordinator coordinator;
|
||||
private readonly TrajectoryExecutor executor;
|
||||
private readonly string sessionId;
|
||||
private long cycleId;
|
||||
|
||||
public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
|
||||
{
|
||||
this.bootstrap = bootstrap ?? throw new ArgumentNullException(nameof(bootstrap));
|
||||
if (!bootstrap.Succeeded)
|
||||
throw new ArgumentException("A successful planning bootstrap is required.", nameof(bootstrap));
|
||||
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
||||
if (planningService == null) throw new ArgumentNullException(nameof(planningService));
|
||||
if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));
|
||||
|
||||
settings.Validate();
|
||||
configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.ReplanPeriodSeconds = settings.ReplanPeriodSeconds;
|
||||
coordinator = new EmPlanningCoordinator(planningService);
|
||||
executor = new TrajectoryExecutor(configuration);
|
||||
this.sessionId = sessionId;
|
||||
}
|
||||
|
||||
public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;
|
||||
|
||||
public bool ShouldStartCycle(DateTimeOffset now)
|
||||
{
|
||||
return coordinator.ShouldStartCycle(now);
|
||||
}
|
||||
|
||||
public Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (state == null) throw new ArgumentNullException(nameof(state));
|
||||
|
||||
const int segmentIndex = 0;
|
||||
long currentCycleId = Interlocked.Increment(ref cycleId);
|
||||
var request = new EmPlanningRequest(
|
||||
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Job.Vehicle, state, configuration,
|
||||
segmentIndex, coordinator.PublishedTrajectory, now, now,
|
||||
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
|
||||
coordinator.PublishedTrajectory?.Metadata.TrajectoryId ?? string.Empty,
|
||||
EmMotionModel.NonholonomicForwardReverse);
|
||||
return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
|
||||
}
|
||||
|
||||
public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
|
||||
{
|
||||
if (state == null) throw new ArgumentNullException(nameof(state));
|
||||
|
||||
EmTrajectory trajectory = coordinator.PublishedTrajectory;
|
||||
if (trajectory == null)
|
||||
return new TrajectoryObservationObservation(now, state, null, null, null, null);
|
||||
|
||||
TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
|
||||
trajectory.Metadata.Direction, trajectory.Metadata.Direction, true);
|
||||
TrajectoryExecutionState executorState = executor.State;
|
||||
return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
|
||||
command, executorState);
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationLsSample
|
||||
{
|
||||
public TrajectoryObservationLsSample(double pathS, double lateralOffset)
|
||||
{
|
||||
PathS = pathS;
|
||||
LateralOffset = lateralOffset;
|
||||
}
|
||||
|
||||
public double PathS { get; }
|
||||
|
||||
public double LateralOffset { get; }
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationStSample
|
||||
{
|
||||
public TrajectoryObservationStSample(double timeFromStart, double pathS)
|
||||
{
|
||||
TimeFromStart = timeFromStart;
|
||||
PathS = pathS;
|
||||
}
|
||||
|
||||
public double TimeFromStart { get; }
|
||||
|
||||
public double PathS { get; }
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationSpeedSample
|
||||
{
|
||||
public TrajectoryObservationSpeedSample(double timeFromStart, double signedLongitudinalVelocity)
|
||||
{
|
||||
TimeFromStart = timeFromStart;
|
||||
SignedLongitudinalVelocity = signedLongitudinalVelocity;
|
||||
}
|
||||
|
||||
public double TimeFromStart { get; }
|
||||
|
||||
public double SignedLongitudinalVelocity { get; }
|
||||
}
|
||||
|
||||
public sealed class TrajectoryObservationCharts
|
||||
{
|
||||
private TrajectoryObservationCharts(IReadOnlyList<TrajectoryObservationLsSample> lsSamples,
|
||||
IReadOnlyList<TrajectoryObservationStSample> stSamples,
|
||||
IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples, int failedProjectionCount)
|
||||
{
|
||||
LsSamples = new ReadOnlyCollection<TrajectoryObservationLsSample>(
|
||||
new List<TrajectoryObservationLsSample>(lsSamples));
|
||||
StSamples = new ReadOnlyCollection<TrajectoryObservationStSample>(
|
||||
new List<TrajectoryObservationStSample>(stSamples));
|
||||
SpeedSamples = new ReadOnlyCollection<TrajectoryObservationSpeedSample>(
|
||||
new List<TrajectoryObservationSpeedSample>(speedSamples));
|
||||
FailedProjectionCount = failedProjectionCount;
|
||||
}
|
||||
|
||||
public IReadOnlyList<TrajectoryObservationLsSample> LsSamples { get; }
|
||||
|
||||
public IReadOnlyList<TrajectoryObservationStSample> StSamples { get; }
|
||||
|
||||
public IReadOnlyList<TrajectoryObservationSpeedSample> SpeedSamples { get; }
|
||||
|
||||
public int FailedProjectionCount { get; }
|
||||
|
||||
public static TrajectoryObservationCharts Build(EmTrajectory trajectory, DirectionSegmentView segment,
|
||||
double maximumProjectionDistanceMeters)
|
||||
{
|
||||
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
||||
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
||||
if (double.IsNaN(maximumProjectionDistanceMeters) || double.IsInfinity(maximumProjectionDistanceMeters) ||
|
||||
maximumProjectionDistanceMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(maximumProjectionDistanceMeters));
|
||||
|
||||
var ls = new List<TrajectoryObservationLsSample>(trajectory.Points.Count);
|
||||
var st = new List<TrajectoryObservationStSample>(trajectory.Points.Count);
|
||||
var speed = new List<TrajectoryObservationSpeedSample>(trajectory.Points.Count);
|
||||
var projector = new FrenetProjector();
|
||||
double seedReferenceS = 0d;
|
||||
int failedProjectionCount = 0;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint point = trajectory.Points[index];
|
||||
var pose = new Pose2D(point.X, point.Y, point.Yaw);
|
||||
if (projector.TryProject(pose, segment, 0d, segment.LengthMeters,
|
||||
maximumProjectionDistanceMeters, seedReferenceS, out FrenetProjection projection))
|
||||
{
|
||||
ls.Add(new TrajectoryObservationLsSample(
|
||||
segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
|
||||
seedReferenceS = projection.ReferenceS;
|
||||
}
|
||||
else
|
||||
{
|
||||
failedProjectionCount++;
|
||||
}
|
||||
|
||||
st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
|
||||
speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
|
||||
}
|
||||
|
||||
return new TrajectoryObservationCharts(ls, st, speed, failedProjectionCount);
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,11 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
@@ -11,6 +16,7 @@ internal static class TrajectoryObservationChecks
|
||||
{
|
||||
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
|
||||
RejectsObstacleOutsideConfiguredBounds();
|
||||
VerifiesLsAndStUsePublishedTrajectoryData();
|
||||
}
|
||||
|
||||
private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
|
||||
@@ -39,6 +45,107 @@ internal static class TrajectoryObservationChecks
|
||||
"observer obstacle outside configured bounds");
|
||||
}
|
||||
|
||||
private static void VerifiesLsAndStUsePublishedTrajectoryData()
|
||||
{
|
||||
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
|
||||
DirectionSegmentView segment = CreateStraightSegment();
|
||||
EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
|
||||
|
||||
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
|
||||
Verification.Equal(2, charts.StSamples.Count, "observer ST sample count");
|
||||
Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time");
|
||||
Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S");
|
||||
Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time");
|
||||
Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S");
|
||||
Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count");
|
||||
Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity,
|
||||
"observer first signed speed");
|
||||
Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity,
|
||||
"observer second signed speed");
|
||||
Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count");
|
||||
Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count");
|
||||
Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
|
||||
Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");
|
||||
|
||||
var settings = new TrajectoryObservationSettings();
|
||||
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
||||
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
||||
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper(
|
||||
new CoarsePathPlanningService(), new PathSmoothingService())
|
||||
.Bootstrap(job, CancellationToken.None);
|
||||
Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds");
|
||||
|
||||
var planningService = new FixedTrajectoryPlanningService(trajectory);
|
||||
var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check");
|
||||
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L);
|
||||
PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None)
|
||||
.GetAwaiter().GetResult();
|
||||
Verification.True(firstCycle.Published, "observer first cycle publishes");
|
||||
Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero");
|
||||
Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId,
|
||||
"observer first trajectory identity");
|
||||
Verification.NearlyEqual(settings.ReplanPeriodSeconds,
|
||||
planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds,
|
||||
"observer configured replan period");
|
||||
|
||||
TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
|
||||
Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
|
||||
"observer executor interpolates from published effective time");
|
||||
|
||||
controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
|
||||
.GetAwaiter().GetResult();
|
||||
Verification.Equal(trajectory, planningService.Requests[1].PreviousTrajectory,
|
||||
"observer rolls published trajectory into next request");
|
||||
Verification.Equal(trajectory.Metadata.TrajectoryId, planningService.Requests[1].PreviousTrajectoryId,
|
||||
"observer rolls published trajectory identity into next request");
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateStraightSegment()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
|
||||
0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward,
|
||||
0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d),
|
||||
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
|
||||
}
|
||||
|
||||
private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt)
|
||||
{
|
||||
var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
|
||||
"observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal);
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
|
||||
TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d,
|
||||
TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private sealed class FixedTrajectoryPlanningService : IEmPlanningService
|
||||
{
|
||||
private readonly EmTrajectory trajectory;
|
||||
|
||||
public FixedTrajectoryPlanningService(EmTrajectory trajectory)
|
||||
{
|
||||
this.trajectory = trajectory;
|
||||
}
|
||||
|
||||
public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();
|
||||
|
||||
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
Requests.Add(request);
|
||||
return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool Throws(Action action)
|
||||
{
|
||||
try { action(); return false; }
|
||||
|
||||
Reference in New Issue
Block a user