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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user