public WayPointsFollowingRRTPlanner(
double goalBias,
int maximumNumberOfIterations,
IVehicleModel vehicleModel,
IMotionModel motionModel,
ITrack track,
ICollisionDetector collisionDetector,
Random random,
TimeSpan timeStep,
IActionSet actions,
IReadOnlyList <IGoal> wayPoints)
{
if (goalBias > 0.5)
{
throw new ArgumentOutOfRangeException($"Goal bias must be at most 0.5 (given {goalBias}).");
}
this.goalBias = goalBias;
this.maximumNumberOfIterations = maximumNumberOfIterations;
this.vehicleModel = vehicleModel;
this.motionModel = motionModel;
this.track = track;
this.collisionDetector = collisionDetector;
this.random = random;
this.timeStep = timeStep;
this.wayPoints = wayPoints;
this.actions = actions;
distances = new DistanceMeasurement(track.Width, track.Height);
wayPointsReached = 0;
ExploredStates = exploredStates;
}
public RRTPlanner(
double goalBias,
int maximumNumberOfIterations,
IWorldDefinition world,
Random random,
TimeSpan timeStep,
IGoal goal)
{
if (goalBias > 0.5)
{
throw new ArgumentOutOfRangeException($"Goal bias must be at most 0.5 (given {goalBias}).");
}
this.goalBias = goalBias;
this.maximumNumberOfIterations = maximumNumberOfIterations;
this.vehicleModel = world.VehicleModel;
this.motionModel = world.MotionModel;
this.track = world.Track;
this.collisionDetector = world.CollisionDetector;
this.random = random;
this.timeStep = timeStep;
this.goal = goal;
this.actions = world.Actions;
distances = new DistanceMeasurement(track.Width, track.Height);
ExploredStates = exploredStates;
}
public PIDAgent(IReadOnlyList <IActionTrajectory> path, IMotionModel motionModel, TimeSpan reactionTime)
{
this.path = path;
this.motionModel = motionModel;
this.reactionTime = reactionTime;
steeringController = new PIDController(0.0395, 0.000155, -0.785);
Visualization = Observable.Merge <IVisualization>(
Observable.Return(new Path(null, path.Select(segment => segment.State.Position).ToArray())),
selectedTarget.Select(point => new Dot(point, 2, "red", reactionTime)),
projectedPosition.Select(point => new Dot(point, 2, "blue", reactionTime)),
crossTrackError.WithLatestFrom(selectedTarget, (error, target) => new Dot(target, Math.Abs(error), Math.Sign(error) == 1 ? "blue" : "red", reactionTime)));
}
public HybridAStarPlanner(
TimeSpan timeStep,
IWorldDefinition world,
IReadOnlyList <IGoal> wayPoints,
bool greedy = false)
{
this.timeStep = timeStep;
this.greedy = greedy;
this.wayPoints = wayPoints;
vehicleModel = world.VehicleModel;
motionModel = world.MotionModel;
track = world.Track;
actions = world.Actions;
collisionDetector = world.CollisionDetector;
discretizer = new StateDiscretizer(
positionXCellSize: vehicleModel.Width / 2,
positionYCellSize: vehicleModel.Width / 2,
headingAngleCellSize: 2 * Math.PI / 36);
ExploredStates = exploredStates;
}
