private ITrackingCommand BuildForwardPass(out bool finalPass)
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Determine Forward Pass");
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
relTransform.TransformPointsInPlace(polygon);
finalOrientation = finalOrientation.Transform(relTransform);
polygonTimestamp = curTimestamp;
// retrieve the vehicle state
Coordinates headingVec = new Coordinates(1, 0);
Coordinates headingVec90 = headingVec.Rotate90();
// check if we can make it out now
Line curLine = new Line(new Coordinates(0,0), headingVec);
Coordinates intersectionPoint;
LineSegment finalLine = finalOrientation;
Circle outCircle = Circle.FromLines(curLine, (Line)finalLine, out intersectionPoint);
double steeringCommand;
if (!outCircle.Equals(Circle.Infinite) && outCircle.r > minRadius) {
// we found an out circle
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "found final pass output");
// build the circle segment
double hitAngle = (intersectionPoint - outCircle.center).ArcTan;
double startAngle = (-outCircle.center).ArcTan;
if (hitAngle < startAngle)
hitAngle += 2*Math.PI;
hitAngle -= startAngle;
if (stopOnLine) {
// get the angle of the end point of the line segment
double endPointAngle = (finalLine.P1 - outCircle.center).ArcTan;
if (endPointAngle < startAngle) endPointAngle += 2*Math.PI;
endPointAngle -= startAngle;
if (endPointAngle < hitAngle) {
hitAngle = endPointAngle;
}
}
// get the obstacles
Coordinates frontLeftPoint = headingVec * TahoeParams.FL + headingVec90 * TahoeParams.T / 2;
Coordinates frontRightPoint = headingVec * TahoeParams.FL - headingVec90 * TahoeParams.T / 2.0;
Coordinates rearRightPoint = -headingVec * TahoeParams.RL - headingVec90 * TahoeParams.T / 2.0;
List<Polygon> obstacles = GetObstacles(curTimestamp);
GetObstacleHitAngle(frontLeftPoint, outCircle.center, obstacles, ref hitAngle);
GetObstacleHitAngle(frontRightPoint, outCircle.center, obstacles, ref hitAngle);
GetObstacleHitAngle(rearRightPoint, outCircle.center, obstacles, ref hitAngle);
// calculate stopping distance
stopDistance = outCircle.r*hitAngle;
stopTimestamp = curTimestamp;
curvature = 1/outCircle.r;
intersectionPoint = outCircle.center + Coordinates.FromAngle(startAngle + hitAngle)*outCircle.r;
// calculate steering angle
steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/outCircle.r, uturnSpeed);
// mark that this will be the final pass
finalPass = true;
Services.UIService.PushCircle(outCircle, curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(intersectionPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
}
else {
finalPass = false;
// draw out 3 circles
// - front right wheel
// - front left wheel
// - rear left wheel
// figure out center point of turn
Circle rearAxleCircle = Circle.FromPointSlopeRadius(Coordinates.Zero, headingVec, minRadius);
Coordinates center = rearAxleCircle.center;
// calculate the points of the wheels
Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2;
Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2;
Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2;
Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2;
// initialize min hit angle to slightly less than 90 degrees
double minHit = Math.PI/2.1;
//GetMinHitAngle(rearLeftPoint, center, true, ref minHit);
//GetMinHitAngle(rearRightPoint, center, true, ref minHit);
GetMinHitAngle(frontLeftPoint, center, ref minHit);
GetMinHitAngle(frontRightPoint, center, ref minHit);
// get the obstacles
List<Polygon> obstacles = GetObstacles(curTimestamp);
frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2;
frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0;
rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0;
GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(frontRightPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);
// trim some off the hit for safety
//if (minHit > 0.5/minRadius)
minHit -= (0.5/minRadius);
minHit = Math.Max(minHit, 0.6 / minRadius);
double startAngle = (Coordinates.Zero-center).ArcTan;
double hitAngle = startAngle + minHit;
// set the stopping point at the min hit point
Coordinates stopPoint = rearAxleCircle.GetPoint(hitAngle);
// calculate the stop distance
stopDistance = minRadius*minHit;
// calculate the required steering angle
steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/minRadius, uturnSpeed);
Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
}
// build the command
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.First);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = forwardLabel;
return cmd;
}
private void ExecutePass()
{
// determine what to do
// get the moving order
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
curTimestamp = pose.timestamp;
ParkerVehicleState state = new ParkerVehicleState(pose.xy, Coordinates.FromAngle(pose.heading));
if (pulloutMode) {
movingOrder = parker.GetNextPulloutOrder(state);
}
else {
movingOrder = parker.GetNextParkingOrder(state);
}
// test for completion
bool isPullinCompleted = false;
if (parkingSpotLine.P0.DistanceTo(pose.xy) < 1.5 && Coordinates.FromAngle(pose.heading).Dot(parkingSpotLine.UnitVector) >= 0.5) {
isPullinCompleted = true;
}
finalPass = false;
if (movingOrder != null && movingOrder.DestState != null && parkingSpotLine.P0.DistanceTo(movingOrder.DestState.Loc) < 1.5 && parkingSpotLine.UnitVector.Dot(movingOrder.DestState.Heading) >= 0.5) {
finalPass = true;
}
if (movingOrder.Completed || (isPullinCompleted && !pulloutMode)) {
Type type;
if (pulloutMode) {
type = typeof(ZoneParkingPullOutBehavior);
}
else {
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
}
else {
// determine the stopping distance
TransmissionGear targetGear;
if (movingOrder.Forward) {
targetGear = TransmissionGear.First;
}
else {
targetGear = TransmissionGear.Reverse;
}
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0)) {
counterClockwise = true;
}
else {
counterClockwise = false;
}
double radius = pose.xy.DistanceTo(movingOrder.CenterPoint);
double startAngle = (pose.xy - movingOrder.CenterPoint).ArcTan;
double endAngle = (movingOrder.DestState.Loc - movingOrder.CenterPoint).ArcTan;
if (counterClockwise) {
if (endAngle < startAngle) {
endAngle += 2*Math.PI;
}
}
else {
if (endAngle > startAngle) {
endAngle -= 2*Math.PI;
}
}
double stopDistance;
if (Math.Abs(radius) < 100) {
stopDistance = Math.Abs(radius * (endAngle - startAngle));
}
else {
stopDistance = pose.xy.DistanceTo(movingOrder.DestState.Loc);
}
this.stopDist = stopDistance;
this.stopTimestamp = curTimestamp;
Services.UIService.PushPoint(movingOrder.DestState.Loc, curTimestamp, "uturn stop point", false);
//double sign = movingOrder.Forward ? 1 : -1;
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/movingOrder.TurningRadius, parking_speed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(targetGear);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), parking_speed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = command_label;
Services.TrackingManager.QueueCommand(cmd);
}
passCompleted = false;
}
private ITrackingCommand BuildBackwardPass()
{
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "UTurn Behavior: Initialize Backward Pass");
// rotate the polygon into the current relative frame
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
RelativeTransform relTransform = Services.RelativePose.GetTransform(polygonTimestamp, curTimestamp);
relTransform.TransformPointsInPlace(polygon);
finalOrientation = finalOrientation.Transform(relTransform);
polygonTimestamp = curTimestamp;
// retrieve the vehicle state
Coordinates headingVec = new Coordinates(1, 0);
Coordinates headingVec180 = headingVec.Rotate180();
Coordinates headingVec90 = headingVec.Rotate90();
// figure out center point of turn
Circle rearAxleCircle = Circle.FromPointSlopeRadius(new Coordinates(0,0), headingVec180, minRadius);
Coordinates center = rearAxleCircle.center;
// calculate the points of the wheels
Coordinates rearLeftPoint = headingVec90*TahoeParams.T/2;
Coordinates rearRightPoint = -headingVec90*TahoeParams.T/2;
Coordinates frontLeftPoint = headingVec*TahoeParams.L + headingVec90*TahoeParams.T/2;
Coordinates frontRightPoint = headingVec*TahoeParams.L - headingVec90*TahoeParams.T/2;
double minHit = Math.PI/2.1;
GetMinHitAngle(rearLeftPoint, center, ref minHit);
GetMinHitAngle(rearRightPoint, center, ref minHit);
//GetMinHitAngle(frontLeftPoint, center, false, ref minHit);
//GetMinHitAngle(frontRightPoint, center, false, ref minHit);
frontLeftPoint = headingVec*TahoeParams.FL + headingVec90*TahoeParams.T/2;
frontRightPoint = headingVec*TahoeParams.FL - headingVec90*TahoeParams.T/2.0;
rearRightPoint = -headingVec*TahoeParams.RL - headingVec90*TahoeParams.T/2.0;
rearLeftPoint = -headingVec*TahoeParams.RL + headingVec90*TahoeParams.T/2.0;
List<Polygon> obstacles = GetObstacles(curTimestamp);
GetObstacleHitAngle(frontLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearLeftPoint, center, obstacles, ref minHit);
GetObstacleHitAngle(rearRightPoint, center, obstacles, ref minHit);
// trim some off the hit for safety'
minHit -= (0.3/minRadius);
// move at least 0.6 meters
minHit = Math.Max(minHit, 0.6 / minRadius);
// calculate the exit stopping point
// shift the line by the minimum turning radius
Coordinates u = finalOrientation.P1 - finalOrientation.P0;
u = u.Normalize().Rotate90();
Line offsetLine = new Line();
offsetLine.P0 = finalOrientation.P0 + u*(minRadius+2);
offsetLine.P1 = finalOrientation.P1 + u*(minRadius+2);
// final the intersection of the current turn circle with a radius of twice the min turn radius and the offset line
Circle twoTurn = new Circle(2*minRadius + 2, center);
Coordinates[] intersections;
double startAngle = (-center).ArcTan;
if (twoTurn.Intersect(offsetLine, out intersections)) {
// figure out where there were hits
for (int i = 0; i < intersections.Length; i++) {
// get the angle of the hit
double angle = (intersections[i] - center).ArcTan;
if (angle < startAngle)
angle += 2*Math.PI;
angle -= startAngle;
if (angle < minHit)
minHit = angle;
}
}
minHit = Math.Max(minHit, 0.6 / minRadius);
// set the stopping point at the min hit point
Coordinates stopPoint = rearAxleCircle.GetPoint(startAngle+minHit);
// calculate the stop distance
stopDistance = rearAxleCircle.r*minHit;
stopTimestamp = curTimestamp;
curvature = 1/minRadius;
// calculate the required steering angle
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(-1/minRadius, uturnSpeed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(TransmissionGear.Reverse);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), uturnSpeed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = backwardLabel;
Services.UIService.PushCircle(new Circle(minRadius, center), curTimestamp, "uturn circle", true);
Services.UIService.PushPoint(stopPoint, curTimestamp, "uturn stop point", true);
Services.UIService.PushPolygon(polygon, curTimestamp, "uturn polygon", true);
return cmd;
}
protected void Track(PlanningResult planningResult, string commandLabel)
{
Services.Dataset.ItemAs<bool>("route feasible").Add(!planningResult.pathBlocked, LocalCarTimeProvider.LocalNow);
if (planningResult.smoothedPath != null && !planningResult.dynamicallyInfeasible) {
prevSmoothedPath = new LinePath(planningResult.smoothedPath);
prevSmoothedPathTimestamp = curTimestamp;
}
if (planningResult.smoothedPath != null) {
Services.UIService.PushLineList(prevSmoothedPath, curTimestamp, "smoothed path", true);
}
if (cancelled) return;
ISpeedCommandGenerator speedCommandGenerator = planningResult.speedCommandGenerator;
ISteeringCommandGenerator steeringCommandGenerator = planningResult.steeringCommandGenerator;
if (speedCommandGenerator == null) {
// we've planned out the path, now build up the command
ISpeedGenerator speedGenerator = null;
if (speedCommand is ScalarSpeedCommand) {
// extract the max speed from the planning settings
double maxSpeed = settings.maxSpeed;
if (planningResult.smoothedPath != null) {
SmoothedPath path = planningResult.smoothedPath;
maxSpeed = Math.Min(maxSpeed, PostProcessSpeed(path));
}
speedGenerator = new ConstantSpeedGenerator(maxSpeed, null);
}
else if (speedCommand is StopAtDistSpeedCommand) {
StopAtDistSpeedCommand stopCommand = (StopAtDistSpeedCommand)speedCommand;
IDistanceProvider distProvider = new TravelledDistanceProvider(behaviorTimestamp, stopCommand.Distance);
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand is StopAtLineSpeedCommand) {
IDistanceProvider distProvider = new StoplineDistanceProvider();
speedGenerator = new StopSpeedGenerator(distProvider, approachSpeed.Value);
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "stay in lane - remaining stop stop dist {0}", distProvider.GetRemainingDistance());
}
else if (speedCommand == null) {
throw new InvalidOperationException("Speed command is null");
}
else {
throw new InvalidOperationException("Speed command " + speedCommand.GetType().FullName + " is not supported");
}
speedCommandGenerator = new FeedbackSpeedCommandGenerator(speedGenerator);
}
if (cancelled) return;
// build up the command
TrackingCommand trackingCommand = new TrackingCommand(speedCommandGenerator, steeringCommandGenerator, false);
trackingCommand.Label = planningResult.commandLabel ?? commandLabel;
// queue it to execute
Services.TrackingManager.QueueCommand(trackingCommand);
}