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();
 }
Esempio n. 3
0
  @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());
 }