355 lines
15 KiB
C#
355 lines
15 KiB
C#
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 readonly VehicleParameters vehicle;
|
|
|
|
private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
|
|
PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
|
|
{
|
|
Job = job ?? throw new ArgumentNullException(nameof(job));
|
|
vehicle = CopyVehicle(job.Vehicle);
|
|
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 VehicleParameters Vehicle => CopyVehicle(vehicle);
|
|
|
|
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);
|
|
}
|
|
|
|
private static VehicleParameters CopyVehicle(VehicleParameters source)
|
|
{
|
|
if (source == null) return null;
|
|
return new VehicleParameters
|
|
{
|
|
LengthMeters = source.LengthMeters,
|
|
WidthMeters = source.WidthMeters,
|
|
SafetyMarginMeters = source.SafetyMarginMeters,
|
|
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
|
|
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
|
|
};
|
|
}
|
|
}
|
|
|
|
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);
|
|
EmTrajectory previousTrajectory = coordinator.PublishedTrajectory;
|
|
var request = new EmPlanningRequest(
|
|
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
|
|
segmentIndex, previousTrajectory, now, now,
|
|
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
|
|
previousTrajectory?.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);
|
|
}
|
|
}
|