OperationalPlan.java

  1. package org.opentrafficsim.core.gtu.plan.operational;

  3. import java.io.Serializable;
  4. import java.util.Arrays;

  6. import org.djunits.value.vdouble.scalar.Acceleration;
  7. import org.djunits.value.vdouble.scalar.Duration;
  8. import org.djunits.value.vdouble.scalar.Length;
  9. import org.djunits.value.vdouble.scalar.Speed;
  10. import org.djunits.value.vdouble.scalar.Time;
  11. import org.djutils.draw.point.OrientedPoint2d;
  12. import org.djutils.draw.point.Point2d;
  13. import org.djutils.exceptions.Throw;
  14. import org.djutils.exceptions.Try;
  15. import org.djutils.immutablecollections.ImmutableList;
  16. import org.opentrafficsim.core.geometry.OtsGeometryException;
  17. import org.opentrafficsim.core.geometry.OtsLine2d;
  18. import org.opentrafficsim.core.gtu.Gtu;
  19. import org.opentrafficsim.core.gtu.RelativePosition;

  21. /**
  22.  * An Operational plan describes a path through the world with a speed profile that a GTU intends to follow. The OperationalPlan
  23.  * can be updated or replaced at any time (including before it has been totally executed), for which a tactical planner is
  24.  * responsible. The operational plan is implemented using segments of the movement (time, location, speed, acceleration) that
  25.  * the GTU will use to plan its location and movement. Within an OperationalPlan the GTU cannot reverse direction along the path
  26.  * of movement. This ensures that the timeAtDistance method will never have to select among several valid solutions.
  27.  * <p>
  28.  * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  29.  * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  30.  * </p>
  31.  * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
  32.  * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
  33.  * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
  34.  */
  35. public class OperationalPlan implements Serializable
  36. {
  37.     /** */
  38.     private static final long serialVersionUID = 20151114L;

  40.     /** The path to follow from a certain time till a certain time. */
  41.     private final OtsLine2d path;

  43.     /** The absolute start time when we start executing the path. */
  44.     private final Time startTime;

  46.     /** The segments that make up the path with an acceleration, constant speed or deceleration profile. */
  47.     private final Segments segments;

  49.     /** The duration of executing the entire operational plan. */
  50.     private final Duration totalDuration;

  52.     /** The length of the entire operational plan. */
  53.     private final Length totalLength;

  55.     /** GTU for debugging purposes. */
  56.     private final Gtu gtu;

  58.     /**
  59.      * An array of relative start times of each segment, expressed in the SI unit, where the last time is the overall ending
  60.      * time of the operational plan.
  61.      */
  62.     private final double[] segmentStartDurations;

  64.     /**
  65.      * An array of relative start distances of each segment, expressed in the SI unit, where the last distance is the overall
  66.      * ending distance of the operational plan.
  67.      */
  68.     private final double[] segmentStartDistances;

  70.     /** The drifting speed. Speeds under this value will be cropped to zero. */
  71.     public static final double DRIFTING_SPEED_SI = 1E-3;

  73.     /**
  74.      * Creates a stand-still plan at a point. A 1m path in the direction of the point is created.
  75.      * @param gtu Gtu; GTU.
  76.      * @param point OrientedPoint2d; point.
  77.      * @param startTime Time; start time.
  78.      * @param duration Duration; duration.
  79.      * @return OperationalPlan; stand-still plan.
  80.      */
  81.     public static OperationalPlan standStill(final Gtu gtu, final OrientedPoint2d point, final Time startTime,
  82.             final Duration duration)
  83.     {
  84.         Point2d p2 = new Point2d(point.x + Math.cos(point.getDirZ()), point.y + Math.sin(point.getDirZ()));
  85.         OtsLine2d path = Try.assign(() -> new OtsLine2d(point, p2), "Unexpected geometry exception.");
  86.         return new OperationalPlan(gtu, path, startTime, Segments.standStill(duration));
  87.     }

  89.     /**
  90.      * Construct an operational plan. The plan will be as long as the minimum of the path or segments allow.
  91.      * @param gtu Gtu; the GTU for debugging purposes
  92.      * @param path OtsLine2d; the path to follow from a certain time till a certain time. The path should have &lt;i&gt;at
  93.      *            least&lt;/i&gt; the length
  94.      * @param startTime Time; the absolute start time when we start executing the path
  95.      * @param segments Segments; the segments that make up the longitudinal dynamics
  96.      */
  97.     public OperationalPlan(final Gtu gtu, final OtsLine2d path, final Time startTime, final Segments segments)
  98.     {
  99.         this.gtu = gtu;
  100.         this.startTime = startTime;
  101.         this.segments = segments;
  102.         this.segmentStartDurations = new double[this.segments.size() + 1];
  103.         this.segmentStartDistances = new double[this.segments.size() + 1];

  105.         Length pathLength = path.getLength();
  106.         Duration segmentsDuration = Duration.ZERO;
  107.         Length segmentsLength = Length.ZERO;
  108.         for (int i = 0; i < this.segments.size(); i++)
  109.         {
  110.             this.segmentStartDurations[i] = segmentsDuration.si;
  111.             this.segmentStartDistances[i] = segmentsLength.si;
  112.             Segment segment = this.segments.get(i);
  113.             segmentsDuration = segmentsDuration.plus(segment.duration());
  114.             segmentsLength = segmentsLength.plus(segment.totalDistance());
  115.         }
  116.         this.segmentStartDurations[this.segments.size()] = segmentsDuration.si;
  117.         this.segmentStartDistances[this.segments.size()] = segmentsLength.si;

  119.         // If segmentsLength == 0, we have a stand-still plan with non-zero length path. This path is required as a degenerate
  120.         // OtsLine2d (with <2 points) is not allowed. In that case (in else) do not truncate path.
  121.         if (segmentsLength.gt0() && pathLength.gt(segmentsLength))
  122.         {
  123.             this.totalDuration = segmentsDuration;
  124.             this.totalLength = segmentsLength;
  125.             this.path = Try.assign(() -> path.extract(0.0, this.totalLength.si), "Unexpected path truncation exception.");
  126.         }
  127.         else if (segmentsLength.gt(pathLength))
  128.         {
  129.             this.totalLength = pathLength;
  130.             int i = this.segments.size();
  131.             while (i > 1 && this.segmentStartDistances[i - 1] > pathLength.si)
  132.             {
  133.                 i--;
  134.             }
  135.             double distanceInLast = this.totalLength.si - this.segmentStartDistances[i - 1];
  136.             Duration timeInLast = this.segments.get(i - 1).durationAtDistance(Length.instantiateSI(distanceInLast));
  137.             this.totalDuration = Duration.instantiateSI(timeInLast.si + this.segmentStartDurations[i - 1]);
  138.             this.path = path;
  139.         }
  140.         else
  141.         {
  142.             this.totalDuration = segmentsDuration;
  143.             this.totalLength = segmentsLength;
  144.             this.path = path;
  145.         }
  146.     }

  148.     /**
  149.      * Return the path that will be traveled. If the plan is a wait plan, the start point of the path is good; the end point of
  150.      * the path is bogus (should only be used to determine the orientation of the GTU).
  151.      * @return OtsLine2d; the path
  152.      */
  153.     public OtsLine2d getPath()
  154.     {
  155.         return this.path;
  156.     }

  158.     /**
  159.      * Return the (absolute) start time of the operational plan.
  160.      * @return Time; startTime
  161.      */
  162.     public Time getStartTime()
  163.     {
  164.         return this.startTime;
  165.     }

  167.     /**
  168.      * Return the start speed of the entire plan.
  169.      * @return Speed; startSpeed
  170.      */
  171.     public Speed getStartSpeed()
  172.     {
  173.         return this.segments.get(0).startSpeed();
  174.     }

  176.     /**
  177.      * Return the segments (parts with constant speed, acceleration or deceleration) of the operational plan.
  178.      * @return ImmutableList&lt;OperationalPlan.Segment&gt;; segmentList
  179.      */
  180.     public ImmutableList<Segment> getOperationalPlanSegmentList()
  181.     {
  182.         return this.segments.getSegments();
  183.     }

  185.     /**
  186.      * Return the time it will take to complete the entire operational plan.
  187.      * @return Duration; the time it will take to complete the entire operational plan
  188.      */
  189.     public Duration getTotalDuration()
  190.     {
  191.         return this.totalDuration;
  192.     }

  194.     /**
  195.      * Return the distance the entire operational plan will cover.
  196.      * @return Length; the distance of the entire operational plan
  197.      */
  198.     public Length getTotalLength()
  199.     {
  200.         return this.totalLength;
  201.     }

  203.     /**
  204.      * Return the time it will take to complete the entire operational plan.
  205.      * @return Time; the time it will take to complete the entire operational plan
  206.      */
  207.     public Time getEndTime()
  208.     {
  209.         return this.startTime.plus(this.totalDuration);
  210.     }

  212.     /**
  213.      * Provide the end location of this operational plan as a DirectedPoint.
  214.      * @return OrientedPoint2d; the end location
  215.      */
  216.     public OrientedPoint2d getEndLocation()
  217.     {
  218.         return Try.assign(() -> this.path.getLocationFraction(Math.min(1.0, this.totalLength.si / this.path.getLength().si)),
  219.                 "Unexpected exception for path extraction till 1.0.");
  220.     }

  222.     /**
  223.      * Returns the index of the segment covering the given time.
  224.      * @param time Time; time.
  225.      * @return int; index of the segment covering the given time.
  226.      */
  227.     private int getSegment(final Time time)
  228.     {
  229.         double duration = time.si - this.startTime.si;
  230.         int segment = 0;
  231.         while (segment < this.segments.size() - 1 && this.segmentStartDurations[segment + 1] < duration)
  232.         {
  233.             segment++;
  234.         }
  235.         return segment;
  236.     }

  238.     /**
  239.      * Return the time when the GTU will reach the given distance.
  240.      * @param distance Length; the distance to calculate the time for
  241.      * @return the time it will take to have traveled the given distance
  242.      */
  243.     public final Time timeAtDistance(final Length distance)
  244.     {
  245.         Throw.when(getTotalLength().lt(distance), IllegalArgumentException.class, "Requesting %s from a plan with length %s",
  246.                 distance, getTotalLength());
  247.         int segment = 0;
  248.         while (segment < this.segments.size() && this.segmentStartDistances[segment + 1] < distance.si)
  249.         {
  250.             segment++;
  251.         }
  252.         Duration durationInSegment = this.segments.get(segment)
  253.                 .durationAtDistance(Length.instantiateSI(distance.si - this.segmentStartDistances[segment]));
  254.         return Time.instantiateSI(this.startTime.si + this.segmentStartDurations[segment] + durationInSegment.si);
  255.     }

  257.     /**
  258.      * Calculate the location after the given duration since the start of the plan.
  259.      * @param duration Duration; the relative time to look for a location
  260.      * @return the location after the given duration since the start of the plan.
  261.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  262.      */
  263.     public final OrientedPoint2d getLocation(final Duration duration) throws OperationalPlanException
  264.     {
  265.         return getLocation(this.startTime.plus(duration));
  266.     }

  268.     /**
  269.      * Calculate the location at the given time.
  270.      * @param time Time; the absolute time to look for a location
  271.      * @return the location at the given time.
  272.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  273.      */
  274.     public final OrientedPoint2d getLocation(final Time time) throws OperationalPlanException
  275.     {
  276.         Throw.when(time.lt(this.startTime), OperationalPlanException.class, "Requested time is before start time.");
  277.         Throw.when(time.gt(this.getEndTime()), OperationalPlanException.class, "Requested time is beyond end time.");
  278.         double fraction = this.totalLength.eq0() ? 0.0 : getTraveledDistance(time).si / this.totalLength.si;
  279.         return Try.assign(() -> this.path.getLocationFraction(fraction, 0.01), OperationalPlanException.class,
  280.                 "Unable to derive location for time.");
  281.     }

  283.     /**
  284.      * Calculate the location after the given duration since the start of the plan.
  285.      * @param time Time; the relative time to look for a location
  286.      * @param pos RelativePosition; relative position
  287.      * @return the location after the given duration since the start of the plan.
  288.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  289.      */
  290.     public final OrientedPoint2d getLocation(final Time time, final RelativePosition pos) throws OperationalPlanException
  291.     {
  292.         double distanceSI = getTraveledDistance(time).si + pos.dx().si;
  293.         return this.path.getLocationExtendedSI(distanceSI);
  294.     }

  296.     /**
  297.      * Calculate the speed of the GTU after the given duration since the start of the plan.
  298.      * @param time Duration; the relative time to look for a location
  299.      * @return the location after the given duration since the start of the plan.
  300.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  301.      */
  302.     public final Speed getSpeed(final Duration time) throws OperationalPlanException
  303.     {
  304.         return getSpeed(time.plus(this.startTime));
  305.     }

  307.     /**
  308.      * Calculate the speed of the GTU at the given time.
  309.      * @param time Time; the absolute time to look for a location
  310.      * @return the location at the given time.
  311.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  312.      */
  313.     public final Speed getSpeed(final Time time) throws OperationalPlanException
  314.     {
  315.         int segment = getSegment(time);
  316.         Duration durationInSegment = Duration.instantiateSI(time.si - this.startTime.si - this.segmentStartDurations[segment]);
  317.         durationInSegment = fixDoublePrecision(durationInSegment, segment);
  318.         return this.segments.get(segment).speed(durationInSegment);
  319.     }

  321.     /**
  322.      * Maximize to segment duration in case of double precision issue.
  323.      * @param durationInSegment Duration; duration in segment.
  324.      * @param segment int; segment number.
  325.      * @return duration in segment, maximized to segment duration if beyond within 1e-6.
  326.      */
  327.     private Duration fixDoublePrecision(final Duration durationInSegment, final int segment)
  328.     {
  329.         if (this.segments.get(segment).duration().lt(durationInSegment)
  330.                 && durationInSegment.si - this.segments.get(segment).duration().si < 1e-6)
  331.         {
  332.             return this.segments.get(segment).duration();
  333.         }
  334.         return durationInSegment;
  335.     }

  337.     /**
  338.      * Calculate the acceleration of the GTU after the given duration since the start of the plan.
  339.      * @param time Duration; the relative time to look for a location
  340.      * @return the location after the given duration since the start of the plan.
  341.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  342.      */
  343.     public final Acceleration getAcceleration(final Duration time) throws OperationalPlanException
  344.     {
  345.         return getAcceleration(time.plus(this.startTime));
  346.     }

  348.     /**
  349.      * Calculate the acceleration of the GTU at the given time.
  350.      * @param time Time; the absolute time to look for a location
  351.      * @return the location at the given time.
  352.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  353.      */
  354.     public final Acceleration getAcceleration(final Time time) throws OperationalPlanException
  355.     {
  356.         return this.segments.get(getSegment(time)).acceleration();
  357.     }

  359.     /**
  360.      * Calculate the distance traveled as part of this plan after the given duration since the start of the plan.
  361.      * @param duration Duration; the relative time to calculate the traveled distance
  362.      * @return the distance traveled as part of this plan after the given duration since the start of the plan.
  363.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  364.      */
  365.     public final Length getTraveledDistance(final Duration duration) throws OperationalPlanException
  366.     {
  367.         return getTraveledDistance(this.startTime.plus(duration));
  368.     }

  370.     /**
  371.      * Calculate the distance traveled as part of this plan at the given absolute time.
  372.      * @param time Time; the absolute time to calculate the traveled distance for as part of this plan
  373.      * @return the distance traveled as part of this plan at the given time
  374.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  375.      */
  376.     public Length getTraveledDistance(final Time time) throws OperationalPlanException
  377.     {
  378.         Throw.when(time.lt(this.getStartTime()), OperationalPlanException.class,
  379.                 "getTravelDistance exception: requested traveled distance before start of plan");
  380.         Throw.when(time.si > this.getEndTime().si + 1e-6, OperationalPlanException.class,
  381.                 "getTravelDistance exception: requested traveled distance beyond end of plan");
  382.         int segment = getSegment(time);
  383.         Duration durationInSegment = Duration.instantiateSI(time.si - this.startTime.si - this.segmentStartDurations[segment]);
  384.         durationInSegment = fixDoublePrecision(durationInSegment, segment);
  385.         double distanceInSegment = this.segments.get(segment).distance(durationInSegment).si;
  386.         return Length.instantiateSI(this.segmentStartDistances[segment] + distanceInSegment);
  387.     }

  389.     /**
  390.      * Calculates when the GTU will be at the given point. The point does not need to be at the traveled path, as the point is
  391.      * projected to the path at 90 degrees. The point may for instance be the end of a lane, which is crossed by a GTU possibly
  392.      * during a lane change.
  393.      * @param point OrientedPoint2d; point with angle, which will be projected to the path at 90 degrees
  394.      * @param upstream boolean; true if the point is upstream of the path
  395.      * @return Time; time at point
  396.      */
  397.     public final Time timeAtPoint(final OrientedPoint2d point, final boolean upstream)
  398.     {
  399.         Point2d p1 = point;
  400.         // point at 90 degrees
  401.         Point2d p2 = new Point2d(point.x - Math.sin(point.getDirZ()), point.y + Math.cos(point.getDirZ()));
  402.         double traveledDistanceAlongPath = 0.0;
  403.         try
  404.         {
  405.             if (upstream)
  406.             {
  407.                 Point2d p = Point2d.intersectionOfLines(this.path.get(0), this.path.get(1), p1, p2);
  408.                 double dist = traveledDistanceAlongPath - this.path.get(0).distance(p);
  409.                 dist = dist >= 0.0 ? dist : 0.0; // negative in case of a gap
  410.                 return timeAtDistance(Length.instantiateSI(dist));
  411.             }
  412.             for (int i = 0; i < this.path.size() - 1; i++)
  413.             {
  414.                 Point2d prevPoint = this.path.get(i);
  415.                 Point2d nextPoint = this.path.get(i + 1);
  416.                 Point2d p = Point2d.intersectionOfLines(prevPoint, nextPoint, p1, p2);
  417.                 if (p == null)
  418.                 {
  419.                     // point too close, check next section
  420.                     continue;
  421.                 }
  422.                 boolean onSegment =
  423.                         prevPoint.distance(nextPoint) + 2e-5 > Math.max(prevPoint.distance(p), nextPoint.distance(p));
  424.                 if (p != null // on segment, or last segment
  425.                         && (i == this.path.size() - 2 || onSegment))
  426.                 {
  427.                     // point is on the line
  428.                     traveledDistanceAlongPath += this.path.get(i).distance(p);
  429.                     if (traveledDistanceAlongPath > this.path.getLength().si)
  430.                     {
  431.                         return Time.instantiateSI(Double.NaN);
  432.                     }
  433.                     return timeAtDistance(Length.instantiateSI(traveledDistanceAlongPath));
  434.                 }
  435.                 else
  436.                 {
  437.                     traveledDistanceAlongPath += this.path.get(i).distance(this.path.get(i + 1));
  438.                 }
  439.             }
  440.         }
  441.         catch (OtsGeometryException exception)
  442.         {
  443.             throw new RuntimeException("Index out of bounds on projection of point to path of operational plan", exception);
  444.         }
  445.         this.gtu.getSimulator().getLogger().always().error("timeAtPoint failed");
  446.         return null;
  447.     }

  449.     /** {@inheritDoc} */
  450.     @SuppressWarnings("checkstyle:designforextension")
  451.     @Override
  452.     public int hashCode()
  453.     {
  454.         final int prime = 31;
  455.         int result = 1;
  456.         result = prime * result + ((this.segments == null) ? 0 : this.segments.hashCode());
  457.         result = prime * result + ((this.path == null) ? 0 : this.path.hashCode());
  458.         result = prime * result + ((this.startTime == null) ? 0 : this.startTime.hashCode());
  459.         return result;
  460.     }

  462.     /** {@inheritDoc} */
  463.     @SuppressWarnings({"checkstyle:needbraces", "checkstyle:designforextension"})
  464.     @Override
  465.     public boolean equals(final Object obj)
  466.     {
  467.         if (this == obj)
  468.             return true;
  469.         if (obj == null)
  470.             return false;
  471.         if (getClass() != obj.getClass())
  472.             return false;
  473.         OperationalPlan other = (OperationalPlan) obj;
  474.         if (this.segments == null)
  475.         {
  476.             if (other.segments != null)
  477.                 return false;
  478.         }
  479.         else if (!this.segments.equals(other.segments))
  480.             return false;
  481.         if (this.path == null)
  482.         {
  483.             if (other.path != null)
  484.                 return false;
  485.         }
  486.         else if (!this.path.equals(other.path))
  487.             return false;
  488.         if (this.startTime == null)
  489.         {
  490.             if (other.startTime != null)
  491.                 return false;
  492.         }
  493.         else if (!this.startTime.equals(other.startTime))
  494.             return false;
  495.         return true;
  496.     }

  498.     /** {@inheritDoc} */
  499.     @SuppressWarnings("checkstyle:designforextension")
  500.     @Override
  501.     public String toString()
  502.     {
  503.         return "OperationalPlan [path=" + this.path + ", startTime=" + this.startTime + ", segments=" + this.segments
  504.                 + ", totalDuration=" + this.totalDuration + ", segmentStartTimesSI="
  505.                 + Arrays.toString(this.segmentStartDurations) + "]";
  506.     }

  508. }
Created with JaCoCo 0.8.12.202403310830