private PlanElement2011 plan(
PlanStackData planData,
SensorData data,
TrackModel trackModel,
OpponentObserver observer,
boolean planning) {
int index = planData.currentSegment();
TrackSegment current = trackModel.getSegment(index);
TrackSegment next = trackModel.getSegment(trackModel.incrementIndex(index));
TrackSegment prev = trackModel.getSegment(trackModel.decrementIndex(index));
double end = planData.end();
double start = planData.start();
if (Plan.TEXT_DEBUG && planning) {
plan.println("");
plan.println("------------ PlanStraight------------");
plan.println(
"Start/End: "
+ Utils.dTS(start)
+ ", "
+ Utils.dTS(end)
+ " ["
+ Utils.dTS(end - start)
+ "m]"
+ " - ");
planData.print();
plan.println("");
}
// plan like the last element?
boolean planLikeLast = current.contains(data.getDistanceFromStartLine());
if (planLikeLast) {
start = data.getDistanceRaced();
if (Plan.TEXT_DEBUG && planning) {
plan.println("This is the last segment to plan for...");
}
}
// check if we can combine the planning of this segment and the previous
if (!planLikeLast && prev.isStraight()) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Previous is also straight, checking further...");
}
if (start - prev.getLength() <= data.getDistanceRaced()) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Prev is the last segment to plan for");
}
start = data.getDistanceRaced();
planLikeLast = true;
} else {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Prev is also a middle segment");
}
start -= prev.getLength();
int prevIndex = trackModel.decrementIndex(index);
int prevPrevIndex = trackModel.decrementIndex(prevIndex);
if (Plan.TEXT_DEBUG && planning) {
plan.println("Switching prev from " + prevIndex + " to " + prevPrevIndex);
}
prev = trackModel.getSegment(prevPrevIndex);
}
if (Plan.TEXT_DEBUG && planning) {
plan.println(
"Moving start to " + Utils.dTS(start) + ", new length " + Utils.dTS(end - start) + "m");
}
planData.popSegment();
}
double brakeDistance = 0.0;
if (planData.approachSpeed != Plan2013.MAX_SPEED) {
if (planData.first()) {
brakeDistance =
plan.calcBrakingZoneStraight(data.getSpeed(), end - start, planData.approachSpeed);
} else {
brakeDistance =
plan.calcBrakingZoneStraight(planData.speed(), end - start, planData.approachSpeed);
}
}
if (Plan.TEXT_DEBUG && planning) {
plan.println("Brake distance: " + Utils.dTS(brakeDistance) + "m");
}
Point2D targetPosition = OpponentObserver.NO_RECOMMENDED_POINT;
if (observer.otherCars()
&& observer.getRecommendedPosition() != OpponentObserver.NO_RECOMMENDED_POINT) {
Point2D recommendation = new Point2D.Double();
recommendation.setLocation(observer.getRecommendedPosition());
if (start <= recommendation.getY() && recommendation.getY() < end) {
targetPosition = new Point2D.Double();
targetPosition.setLocation(recommendation);
}
}
// track positions
ArrayList<Interpolator> positions = new ArrayList<>();
double currStart = start;
double remainingLength = end - start;
double currPosition = plan.getAnchorPoint(prev, current);
if (planLikeLast) {
currPosition = data.getTrackPosition();
}
if (Plan.TEXT_DEBUG && planning) {
plan.println(Utils.dTS(remainingLength) + "m remain...");
}
if (targetPosition != OpponentObserver.NO_RECOMMENDED_POINT) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("I need to take care of other cars, point is " + targetPosition.toString());
plan.println("Right now, i'm planning with currStart: " + Utils.dTS(currStart));
}
if (targetPosition.getY() < currStart) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Point is before currStart, moving it to +1m");
}
targetPosition.setLocation(targetPosition.getX(), targetPosition.getY() + 1.0);
}
if (Plan.TEXT_DEBUG && planning) {
plan.println("Checking, if there is enough room...");
}
double lengthNeeded = targetPosition.getY() - currStart;
if (Plan.TEXT_DEBUG && planning) {
plan.println(
"I need at least "
+ Utils.dTS(lengthNeeded)
+ "m, "
+ Utils.dTS(remainingLength)
+ "m remain");
}
if (remainingLength < lengthNeeded) {
targetPosition = OpponentObserver.NO_RECOMMENDED_POINT;
if (Plan.TEXT_DEBUG && planning) {
plan.println("Cannot overtake, not enough room");
}
}
}
if (targetPosition != OpponentObserver.NO_RECOMMENDED_POINT) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Trying to overtake");
}
// switch Position 1
double[] xP = new double[3];
double[] yP = new double[3];
xP[0] = currStart;
xP[2] = targetPosition.getY();
xP[1] = (xP[0] + xP[2]) / 2.0;
yP[0] = currPosition;
yP[2] = targetPosition.getX();
yP[1] = (yP[0] + yP[2]) / 2.0;
if (Plan.TEXT_DEBUG && planning) {
plan.println("Switch Position 1:");
for (int k = 0; k < xP.length; ++k) {
plan.println(xP[k] + " , " + yP[k]);
}
}
CubicSpline spline = new CubicSpline(xP, yP);
spline.setDerivLimits(0.0, 0.0);
positions.add(new FlanaganCubicWrapper(spline));
currStart = xP[2];
remainingLength = end - currStart;
currPosition = targetPosition.getX();
// overtaking
if (Plan.TEXT_DEBUG && planning) {
plan.println("Overtaking line");
}
xP = new double[3];
xP[0] = currStart;
xP[2] = end - 150.0;
if (Plan.TEXT_DEBUG && planning) {
plan.println(
"Overtaking line: "
+ Utils.dTS(currStart)
+ " "
+ Utils.dTS(end - 150.0)
+ " "
+ targetPosition.getX());
}
positions.add(new ConstantValue(xP[0], xP[2], targetPosition.getX()));
currStart = xP[2];
remainingLength = end - currStart;
currPosition = targetPosition.getX();
}
// simply drive towards the target position for the next corner
if (Plan.TEXT_DEBUG && planning) {
plan.println("Planning towards the next corner...");
}
double[] xP = new double[3];
double[] yP = new double[3];
xP[0] = currStart;
xP[2] = end;
xP[1] = (xP[0] + xP[2]) / 2.0;
yP[0] = currPosition;
double absCurrPosition =
SensorData.calcAbsoluteTrackPosition(currPosition, trackModel.getWidth());
double possibleDelta = Plan2013.calcPossibleSwitchDelta(data, end - currStart);
double anchor = plan.getAnchorPoint(current, next);
double absDesiredPosition = SensorData.calcAbsoluteTrackPosition(anchor, trackModel.getWidth());
if (Plan.TEXT_DEBUG && planning) {
plan.println("absCurrPosition: " + Utils.dTS(absCurrPosition));
plan.println("PossibleDelta: " + Utils.dTS(possibleDelta));
plan.println("absDesiredPosition: " + Utils.dTS(absDesiredPosition));
}
if (Math.abs(absDesiredPosition - absCurrPosition) <= possibleDelta) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Positioning ok");
}
yP[2] = plan.getAnchorPoint(current, next);
} else {
// move to the right
if (anchor < 0) {
yP[2] =
SensorData.calcRelativeTrackPosition(
absCurrPosition + possibleDelta, trackModel.getWidth());
} else {
// move to the left
yP[2] =
SensorData.calcRelativeTrackPosition(
absCurrPosition - possibleDelta, trackModel.getWidth());
}
}
yP[1] = (yP[0] + yP[2]) / 2.0;
if (Plan.TEXT_DEBUG && planning) {
plan.println("Position towards the next corner:");
for (int k = 0; k < xP.length; ++k) {
plan.println(xP[k] + " , " + yP[k]);
}
}
CubicSpline spline = new CubicSpline(xP, yP);
spline.setDerivLimits(0.0, 0.0);
positions.add(new FlanaganCubicWrapper(spline));
// target speed
double[] xS;
double[] yS;
if (brakeDistance >= end - start || brakeDistance == 0.0) {
xS = new double[3];
yS = new double[3];
xS[0] = start;
xS[2] = end;
xS[1] = (xS[0] + xS[2]) / 2.0;
if (brakeDistance == 0.0) {
yS[0] = Plan2013.MAX_SPEED;
yS[1] = Plan2013.MAX_SPEED;
yS[2] = Plan2013.MAX_SPEED;
planData.approachSpeed = Plan2013.MAX_SPEED;
} else {
yS[0] = planData.approachSpeed;
yS[1] = planData.approachSpeed;
yS[2] = planData.approachSpeed;
planData.approachSpeed =
plan.calcApproachSpeedStraight(planData.approachSpeed, end - start);
}
} else {
xS = new double[4];
yS = new double[4];
xS[0] = start;
xS[1] = end - brakeDistance;
xS[2] = end - (brakeDistance * 0.99);
xS[3] = end;
yS[0] = Plan2013.MAX_SPEED;
yS[1] = Plan2013.MAX_SPEED;
yS[2] = planData.approachSpeed;
yS[3] = planData.approachSpeed;
planData.approachSpeed = Plan2013.MAX_SPEED;
}
if (Plan.TEXT_DEBUG && planning) {
plan.println("Speed:");
for (int i = 0; i < xS.length; ++i) {
plan.println(xS[i] + " , " + yS[i]);
}
}
LinearInterpolator speed;
try {
speed = new LinearInterpolator(xS, yS);
} catch (RuntimeException e) {
System.out.println("*****************EXCEPTION**************");
System.out.println(
"Start/End: "
+ Utils.dTS(start)
+ ", "
+ Utils.dTS(end)
+ " ["
+ Utils.dTS(end - start)
+ "m]"
+ " - ");
System.out.println("Segment: " + current.toString());
System.out.println("Start: " + start);
System.out.println("End: " + end);
System.out.println("BrakeDistance: " + brakeDistance);
System.out.println("Data:");
try {
java.io.OutputStreamWriter osw = new java.io.OutputStreamWriter(System.out);
SensorData.writeHeader(osw);
osw.append('\n');
data.write(osw);
osw.flush();
} catch (Exception schwupp) {
}
System.out.println("");
System.out.println("Speed:");
for (int i = 0; i < xS.length; ++i) {
System.out.println(xS[i] + " , " + yS[i]);
}
System.out.println("Complete Model:");
trackModel.print();
throw e;
}
PlanElement2011 element = new PlanElement2011(xS[0], xS[xS.length - 1], "Accelerate");
for (Interpolator cs : positions) {
element.attachPosition(cs);
}
element.setSpeed(speed);
return element;
}
@Override
public String toString() {
return myLocation.toString();
}
@Override
public String getTrajectoryDescription() {
StringBuilder sb = new StringBuilder();
Point2D last = null;
double lastOrientation = 0;
double lastTimestamp = 0;
Map<String, String> params = new HashMap<String, String>();
for (KeyFrame frame : frames) {
writeLine(sb, frame);
if (frame.hasComments()) {
ArrayList<String> comments = frame.getComments();
for (String s : comments) {
addParameters(params, s);
}
}
TrajectoryMovement movement = frame.getMovement();
switch (movement) {
case START:
last = frame.getPosition();
lastOrientation = frame.getOrientation();
sb.append(String.format("// start at %s\r\n", last.toString()));
break;
case BEZIER:
break;
case CLOTHOID:
break;
case LINE:
Point2D p = frame.getPosition();
double distance = p.distance(last);
double speed = frame.getMovementSpeed();
String direction = speed > 0 ? "forward" : "backward";
addComments(
params,
String.format(
"// move %s of %.0f mm (%.0f)",
direction, distance, 9.557 * distance * Math.signum(speed)),
true);
addValue(params, KEY_DISTANCE, 9.557 * distance * Math.signum(speed));
last = p;
break;
case NONE:
// double delay = frame.getTimestamp() - lastTimestamp;
// addComments(params, String.format("delay_ms(%.0f);", delay * 1000), true);
break;
case ROTATION:
double o = frame.getOrientation();
double angle = Angle.getRotationAngle(lastOrientation, o);
if (frame.useRelativeAngle()) {
// the angle is a relative angle that can be > 180°
angle = frame.getRelativeAngle();
}
angle = Math.toDegrees(angle);
addComments(
params,
String.format("// rotation of %.0f° (%.0f)", angle, 22527.5d / 360d * angle),
true);
addValue(params, KEY_ANGLE, 22527.5d / 360d * angle);
if (params.containsKey(KEY_ORIENTATION)) {
double opposite = getDoubleValue(params, KEY_ORIENTATION);
opposite = Math.toDegrees(opposite);
addValue(params, KEY_ORIENTATION, 22527.5d / 360d * opposite);
}
lastOrientation = o;
break;
default:
break;
}
lastTimestamp = frame.getTimestamp();
writeCommands(sb, params);
}
return sb.toString();
}
private static void print(String msg, Point2D p) {
System.out.println(msg + p.toString());
}