DirectInfrastructurePerception.java

  1. package org.opentrafficsim.road.gtu.lane.perception.categories;

  3. import java.util.LinkedHashMap;
  4. import java.util.LinkedHashSet;
  5. import java.util.Map;
  6. import java.util.Objects;
  7. import java.util.Set;
  8. import java.util.SortedSet;
  9. import java.util.TreeSet;
  10. import java.util.WeakHashMap;

  12. import org.djunits.value.vdouble.scalar.Length;
  13. import org.djutils.exceptions.Throw;
  14. import org.djutils.exceptions.Try;
  15. import org.opentrafficsim.base.TimeStampedObject;
  16. import org.opentrafficsim.base.parameters.ParameterException;
  17. import org.opentrafficsim.core.gtu.GTUException;
  18. import org.opentrafficsim.core.gtu.RelativePosition;
  19. import org.opentrafficsim.core.network.LateralDirectionality;
  20. import org.opentrafficsim.core.network.NetworkException;
  21. import org.opentrafficsim.core.network.route.Route;
  22. import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
  23. import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
  24. import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
  25. import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
  26. import org.opentrafficsim.road.network.lane.Lane;
  27. import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
  28. import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
  29. import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
  30. import org.opentrafficsim.road.network.speed.SpeedLimitTypes;

  32. /**
  33.  * Perceives information concerning the infrastructure, including splits, lanes, speed limits and road markings. This category
  34.  * is optimized by cooperating closely with the {@code LaneStructure} and only updating internal information when the GTU is on
  35.  * a new {@code Lane}. On the {@code Lane} information is defined relative to the start, and thus easily calculated at each
  36.  * time.
  37.  * <p>
  38.  * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  39.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  40.  * <p>
  41.  * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jul 14, 2016 <br>
  42.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  43.  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  44.  * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  45.  */
  46. // TODO: more than the lane speed limit and maximum vehicle speed in the speed limit prospect
  47. public class DirectInfrastructurePerception extends LaneBasedAbstractPerceptionCategory implements InfrastructurePerception
  48. {

  50.     /** */
  51.     private static final long serialVersionUID = 20160811L;

  53.     /** Infrastructure lane change info per relative lane. */
  54.     private final Map<RelativeLane, TimeStampedObject<SortedSet<InfrastructureLaneChangeInfo>>> infrastructureLaneChangeInfo =
  55.             new LinkedHashMap<>();

  57.     /** Speed limit prospect per relative lane. */
  58.     private Map<RelativeLane, TimeStampedObject<SpeedLimitProspect>> speedLimitProspect = new LinkedHashMap<>();

  60.     /** Legal Lane change possibilities per relative lane and lateral direction. */
  61.     private final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<
  62.             LaneChangePossibility>>> legalLaneChangePossibility = new LinkedHashMap<>();

  64.     /** Physical Lane change possibilities per relative lane and lateral direction. */
  65.     private final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<
  66.             LaneChangePossibility>>> physicalLaneChangePossibility = new LinkedHashMap<>();

  68.     /** Cross-section. */
  69.     private TimeStampedObject<SortedSet<RelativeLane>> crossSection;

  71.     /** Cache for anyNextOk. */
  72.     private final Map<LaneStructureRecord, Boolean> anyNextOkCache = new WeakHashMap<>();

  74.     /** Set of records with accessible end as they are cut off. */
  75.     private final Set<LaneStructureRecord> cutOff = new LinkedHashSet<>();

  77.     /** Root. */
  78.     private LaneStructureRecord root;

  80.     /** Lanes registered to the GTU used to check if an update is required. */
  81.     private Set<Lane> lanes;

  83.     /** Route. */
  84.     private Route route;

  86.     /**
  87.      * @param perception LanePerception; perception
  88.      */
  89.     public DirectInfrastructurePerception(final LanePerception perception)
  90.     {
  91.         super(perception);
  92.     }

  94.     /** {@inheritDoc} */
  95.     @Override
  96.     public void updateAll() throws GTUException, ParameterException
  97.     {
  98.         updateCrossSection();
  99.         // clean-up
  100.         Set<RelativeLane> cs = getCrossSection();
  101.         this.infrastructureLaneChangeInfo.keySet().retainAll(cs);
  102.         this.legalLaneChangePossibility.keySet().retainAll(cs);
  103.         this.physicalLaneChangePossibility.keySet().retainAll(cs);
  104.         this.speedLimitProspect.keySet().retainAll(cs);
  105.         // only if required
  106.         LaneStructureRecord newRoot = getPerception().getLaneStructure().getRootRecord();
  107.         if (this.root == null || !newRoot.equals(this.root) || !this.lanes.equals(getPerception().getGtu().positions(
  108.             RelativePosition.REFERENCE_POSITION).keySet()) || !Objects.equals(this.route, getPerception().getGtu()
  109.                 .getStrategicalPlanner().getRoute()) || this.cutOff.stream().filter((record) -> !record.isCutOffEnd())
  110.                     .count() > 0)
  111.         {
  112.             this.cutOff.clear();
  113.             this.root = newRoot;
  114.             this.lanes = getPerception().getGtu().positions(RelativePosition.REFERENCE_POSITION).keySet();
  115.             this.route = getPerception().getGtu().getStrategicalPlanner().getRoute();
  116.             // TODO: this is not suitable if we change lane and consider e.g. dynamic speed signs, they will be forgotten
  117.             this.speedLimitProspect.clear();
  118.             for (RelativeLane lane : getCrossSection())
  119.             {
  120.                 updateInfrastructureLaneChangeInfo(lane);
  121.                 updateLegalLaneChangePossibility(lane, LateralDirectionality.LEFT);
  122.                 updateLegalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
  123.                 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.LEFT);
  124.                 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
  125.             }
  126.         }

  128.         // speed limit prospect
  129.         for (RelativeLane lane : getCrossSection())
  130.         {
  131.             updateSpeedLimitProspect(lane);
  132.         }
  133.         for (RelativeLane lane : getCrossSection())
  134.         {
  135.             if (!this.infrastructureLaneChangeInfo.containsKey(lane))
  136.             {
  137.                 updateInfrastructureLaneChangeInfo(lane); // new lane in cross section
  138.                 updateLegalLaneChangePossibility(lane, LateralDirectionality.LEFT);
  139.                 updateLegalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
  140.                 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.LEFT);
  141.                 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
  142.             }
  143.         }
  144.     }

  146.     /** {@inheritDoc} */
  147.     @Override
  148.     public final void updateInfrastructureLaneChangeInfo(final RelativeLane lane) throws GTUException, ParameterException
  149.     {
  150.         if (this.infrastructureLaneChangeInfo.containsKey(lane) && this.infrastructureLaneChangeInfo.get(lane).getTimestamp()
  151.             .equals(getTimestamp()))
  152.         {
  153.             // already done at this time
  154.             return;
  155.         }
  156.         updateCrossSection();

  158.         // start at requested lane
  159.         SortedSet<InfrastructureLaneChangeInfo> resultSet = new TreeSet<>();
  160.         LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
  161.         try
  162.         {
  163.             record = getPerception().getLaneStructure().getFirstRecord(lane);
  164.             if (!record.allowsRoute(getGtu().getStrategicalPlanner().getRoute(), getGtu().getGTUType()))
  165.             {
  166.                 resultSet.add(InfrastructureLaneChangeInfo.fromInaccessibleLane(record.isDeadEnd()));
  167.                 this.infrastructureLaneChangeInfo.put(lane, new TimeStampedObject<>(resultSet, getTimestamp()));
  168.                 return;
  169.             }
  170.         }
  171.         catch (NetworkException exception)
  172.         {
  173.             throw new GTUException("Route has no destination.", exception);
  174.         }
  175.         Map<LaneStructureRecord, InfrastructureLaneChangeInfo> currentSet = new LinkedHashMap<>();
  176.         Map<LaneStructureRecord, InfrastructureLaneChangeInfo> nextSet = new LinkedHashMap<>();
  177.         RelativePosition front = getPerception().getGtu().getFront();
  178.         currentSet.put(record, new InfrastructureLaneChangeInfo(0, record, front, record.isDeadEnd(),
  179.             LateralDirectionality.NONE));
  180.         while (!currentSet.isEmpty())
  181.         {
  182.             // move lateral
  183.             nextSet.putAll(currentSet);
  184.             for (LaneStructureRecord laneRecord : currentSet.keySet())
  185.             {
  186.                 while (laneRecord.legalLeft() && !nextSet.containsKey(laneRecord.getLeft()))
  187.                 {
  188.                     InfrastructureLaneChangeInfo info = nextSet.get(laneRecord).left(laneRecord.getLeft(), front, laneRecord
  189.                         .getLeft().isDeadEnd());
  190.                     nextSet.put(laneRecord.getLeft(), info);
  191.                     laneRecord = laneRecord.getLeft();
  192.                 }
  193.             }
  194.             for (LaneStructureRecord laneRecord : currentSet.keySet())
  195.             {
  196.                 while (laneRecord.legalRight() && !nextSet.containsKey(laneRecord.getRight()))
  197.                 {
  198.                     InfrastructureLaneChangeInfo info = nextSet.get(laneRecord).right(laneRecord.getRight(), front, laneRecord
  199.                         .getRight().isDeadEnd());
  200.                     nextSet.put(laneRecord.getRight(), info);
  201.                     laneRecord = laneRecord.getRight();
  202.                 }
  203.             }
  204.             // move longitudinal
  205.             currentSet = nextSet;
  206.             nextSet = new LinkedHashMap<>();
  207.             InfrastructureLaneChangeInfo bestOk = null;
  208.             InfrastructureLaneChangeInfo bestNotOk = null;
  209.             boolean deadEnd = false;
  210.             for (LaneStructureRecord laneRecord : currentSet.keySet())
  211.             {
  212.                 boolean anyOk = Try.assign(() -> anyNextOk(laneRecord), "Route has no destination.");
  213.                 if (anyOk)
  214.                 {
  215.                     // add to nextSet
  216.                     for (LaneStructureRecord next : laneRecord.getNext())
  217.                     {
  218.                         try
  219.                         {
  220.                             if (next.allowsRoute(getGtu().getStrategicalPlanner().getRoute(), getGtu().getGTUType()))
  221.                             {
  222.                                 InfrastructureLaneChangeInfo prev = currentSet.get(laneRecord);
  223.                                 InfrastructureLaneChangeInfo info = new InfrastructureLaneChangeInfo(prev
  224.                                     .getRequiredNumberOfLaneChanges(), next, front, next.isDeadEnd(), prev
  225.                                         .getLateralDirectionality());
  226.                                 nextSet.put(next, info);
  227.                             }
  228.                         }
  229.                         catch (NetworkException exception)
  230.                         {
  231.                             throw new RuntimeException("Network exception while considering route on next lane.", exception);
  232.                         }
  233.                     }
  234.                     // take best ok
  235.                     if (bestOk == null || currentSet.get(laneRecord).getRequiredNumberOfLaneChanges() < bestOk
  236.                         .getRequiredNumberOfLaneChanges())
  237.                     {
  238.                         bestOk = currentSet.get(laneRecord);
  239.                     }
  240.                 }
  241.                 else
  242.                 {
  243.                     // take best not ok
  244.                     deadEnd = deadEnd || currentSet.get(laneRecord).isDeadEnd();
  245.                     if (bestNotOk == null || currentSet.get(laneRecord).getRequiredNumberOfLaneChanges() < bestNotOk
  246.                         .getRequiredNumberOfLaneChanges())
  247.                     {
  248.                         bestNotOk = currentSet.get(laneRecord);
  249.                     }
  250.                 }

  252.             }
  253.             if (bestOk == null)
  254.             {
  255.                 break;
  256.             }
  257.             // if there are lanes that are not okay and only -further- lanes that are ok, we need to change to one of the ok's
  258.             if (bestNotOk != null && bestOk.getRequiredNumberOfLaneChanges() > bestNotOk.getRequiredNumberOfLaneChanges())
  259.             {
  260.                 bestOk.setDeadEnd(deadEnd);
  261.                 resultSet.add(bestOk);
  262.             }
  263.             currentSet = nextSet;
  264.             nextSet = new LinkedHashMap<>();
  265.         }

  267.         // save
  268.         this.infrastructureLaneChangeInfo.put(lane, new TimeStampedObject<>(resultSet, getTimestamp()));
  269.     }

  271.     /**
  272.      * Returns whether the given record end is ok to pass. If not, a lane change is required before this end. The method will
  273.      * also return true if the next node is the end node of the route, if the lane is cut off due to limited perception range,
  274.      * or when there is a {@code SinkSensor} on the lane.
  275.      * @param record LaneStructureRecord; checked record
  276.      * @return whether the given record end is ok to pass
  277.      * @throws NetworkException if destination could not be obtained
  278.      * @throws GTUException if the GTU could not be obtained
  279.      */
  280.     private boolean anyNextOk(final LaneStructureRecord record) throws NetworkException, GTUException
  281.     {
  282.         if (record.isCutOffEnd())
  283.         {
  284.             this.cutOff.add(record);
  285.             return true; // always ok if cut-off
  286.         }
  287.         // check cache
  288.         Boolean ok = this.anyNextOkCache.get(record);
  289.         if (ok != null)
  290.         {
  291.             return ok;
  292.         }
  293.         // sink
  294.         for (SingleSensor s : record.getLane().getSensors())
  295.         {
  296.             // XXX for now, we do allow to lower speed for a DestinationSensor (e.g., to brake for parking)
  297.             if (s instanceof SinkSensor)
  298.             {
  299.                 this.anyNextOkCache.put(record, true);
  300.                 return true; // ok towards sink
  301.             }
  302.         }
  303.         // check destination
  304.         Route currentRoute = getGtu().getStrategicalPlanner().getRoute();
  305.         try
  306.         {
  307.             if (currentRoute != null && currentRoute.destinationNode().equals(record.getToNode()))
  308.             {
  309.                 this.anyNextOkCache.put(record, true);
  310.                 return true;
  311.             }
  312.         }
  313.         catch (NetworkException exception)
  314.         {
  315.             throw new RuntimeException("Could not determine destination node.", exception);
  316.         }
  317.         // check dead-end
  318.         if (record.getNext().isEmpty())
  319.         {
  320.             this.anyNextOkCache.put(record, false);
  321.             return false; // never ok if dead-end
  322.         }
  323.         // check if we have a route
  324.         if (currentRoute == null)
  325.         {
  326.             this.anyNextOkCache.put(record, true);
  327.             return true; // if no route assume ok, i.e. simple networks without routes
  328.         }
  329.         // finally check route
  330.         ok = record.allowsRouteAtEnd(currentRoute, getGtu().getGTUType());
  331.         this.anyNextOkCache.put(record, ok);
  332.         return ok;
  333.     }

  335.     /** {@inheritDoc} */
  336.     @Override
  337.     public final void updateSpeedLimitProspect(final RelativeLane lane) throws GTUException, ParameterException
  338.     {
  339.         updateCrossSection();
  340.         checkLaneIsInCrossSection(lane);
  341.         TimeStampedObject<SpeedLimitProspect> tsSlp = this.speedLimitProspect.get(lane);
  342.         SpeedLimitProspect slp;
  343.         if (tsSlp != null)
  344.         {
  345.             slp = tsSlp.getObject();
  346.             slp.update(getGtu().getOdometer());
  347.         }
  348.         else
  349.         {
  350.             slp = new SpeedLimitProspect(getGtu().getOdometer());
  351.             slp.addSpeedInfo(Length.ZERO, SpeedLimitTypes.MAX_VEHICLE_SPEED, getGtu().getMaximumSpeed(), getGtu());
  352.         }
  353.         try
  354.         {
  355.             Lane laneObj = getGtu().getReferencePosition().getLane();
  356.             if (!slp.containsAddSource(laneObj))
  357.             {
  358.                 slp.addSpeedInfo(Length.ZERO, SpeedLimitTypes.FIXED_SIGN, laneObj.getSpeedLimit(getGtu().getGTUType()),
  359.                     laneObj);
  360.             }
  361.         }
  362.         catch (NetworkException exception)
  363.         {
  364.             throw new RuntimeException("Could not obtain speed limit from lane for perception.", exception);
  365.         }
  366.         this.speedLimitProspect.put(lane, new TimeStampedObject<>(slp, getTimestamp()));
  367.     }

  369.     /** {@inheritDoc} */
  370.     @Override
  371.     public final void updateLegalLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat)
  372.             throws GTUException, ParameterException
  373.     {
  374.         updateLaneChangePossibility(lane, lat, true, this.legalLaneChangePossibility);
  375.     }

  377.     /** {@inheritDoc} */
  378.     @Override
  379.     public final void updatePhysicalLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat)
  380.             throws GTUException, ParameterException
  381.     {
  382.         updateLaneChangePossibility(lane, lat, false, this.physicalLaneChangePossibility);
  383.     }

  385.     /**
  386.      * Updates the distance over which lane changes remains legally or physically possible.
  387.      * @param lane RelativeLane; lane from which the lane change possibility is requested
  388.      * @param lat LateralDirectionality; LEFT or RIGHT, null not allowed
  389.      * @param legal boolean; legal, or physical otherwise
  390.      * @param possibilityMap
  391.      *            Map&lt;RelativeLane,Map&lt;LateralDirectionality,TimeStampedObject&lt;LaneChangePossibility&gt;&gt;&gt;;
  392.      *            Map&lt;RelativeLane,Map&lt;LateralDirectionality,TimeStampedObject&lt;LaneChangePossibility&gt;&gt;&gt;; legal
  393.      *            or physical possibility map
  394.      * @throws GTUException if the GTU was not initialized or if the lane is not in the cross section
  395.      * @throws ParameterException if a parameter is not defined
  396.      */
  397.     private void updateLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat, final boolean legal,
  398.             final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<LaneChangePossibility>>> possibilityMap)
  399.             throws GTUException, ParameterException
  400.     {
  401.         updateCrossSection();
  402.         checkLaneIsInCrossSection(lane);

  404.         if (possibilityMap.get(lane) == null)
  405.         {
  406.             possibilityMap.put(lane, new LinkedHashMap<>());
  407.         }
  408.         LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
  409.         // check tail
  410.         Length tail = getPerception().getGtu().getRear().getDx();
  411.         while (record != null && record.getStartDistance().gt(tail) && !record.getPrev().isEmpty() && ((lat.isLeft() && record
  412.             .possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal))))
  413.         {
  414.             if (record.getPrev().size() > 1)
  415.             {
  416.                 // assume not possible at a merge
  417.                 possibilityMap.get(lane).put(lat, new TimeStampedObject<>(new LaneChangePossibility(record.getPrev().get(0),
  418.                     tail, true), getTimestamp()));
  419.                 return;
  420.             }
  421.             else if (record.getPrev().isEmpty())
  422.             {
  423.                 // dead-end, no lane upwards prevents a lane change
  424.                 break;
  425.             }
  426.             record = record.getPrev().get(0);
  427.             if ((lat.isLeft() && !record.possibleLeft(legal)) || (lat.isRight() && !record.possibleRight(legal)))
  428.             {
  429.                 // this lane prevents a lane change for the tail
  430.                 possibilityMap.get(lane).put(lat, new TimeStampedObject<>(new LaneChangePossibility(record, tail, true),
  431.                     getTimestamp()));
  432.                 return;
  433.             }
  434.         }

  436.         LaneStructureRecord prevRecord = null;
  437.         record = getPerception().getLaneStructure().getFirstRecord(lane);

  439.         Length dx;
  440.         if ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal)))
  441.         {
  442.             dx = getPerception().getGtu().getFront().getDx();
  443.             while (record != null && ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(
  444.                 legal))))
  445.             {
  446.                 // TODO: splits
  447.                 prevRecord = record;
  448.                 record = record.getNext().isEmpty() ? null : record.getNext().get(0);
  449.             }
  450.         }
  451.         else
  452.         {
  453.             dx = getPerception().getGtu().getRear().getDx();
  454.             while (record != null && ((lat.isLeft() && !record.possibleLeft(legal)) || (lat.isRight() && !record.possibleRight(
  455.                 legal))))
  456.             {
  457.                 // TODO: splits
  458.                 prevRecord = record;
  459.                 record = record.getNext().isEmpty() ? null : record.getNext().get(0);
  460.             }
  461.         }
  462.         possibilityMap.get(lane).put(lat, new TimeStampedObject<>(new LaneChangePossibility(prevRecord, dx, true),
  463.             getTimestamp()));
  464.     }

  466.     /**
  467.      * @param lane RelativeLane; lane to check
  468.      * @throws GTUException if the lane is not in the cross section
  469.      */
  470.     private void checkLaneIsInCrossSection(final RelativeLane lane) throws GTUException
  471.     {
  472.         Throw.when(!getCrossSection().contains(lane), GTUException.class,
  473.             "The requeasted lane %s is not in the most recent cross section.", lane);
  474.     }

  476.     /** {@inheritDoc} */
  477.     @Override
  478.     public final void updateCrossSection() throws GTUException, ParameterException
  479.     {
  480.         if (this.crossSection != null && this.crossSection.getTimestamp().equals(getTimestamp()))
  481.         {
  482.             // already done at this time
  483.             return;
  484.         }
  485.         this.crossSection = new TimeStampedObject<>(getPerception().getLaneStructure().getExtendedCrossSection(),
  486.             getTimestamp());
  487.     }

  489.     /** {@inheritDoc} */
  490.     @Override
  491.     public final SortedSet<InfrastructureLaneChangeInfo> getInfrastructureLaneChangeInfo(final RelativeLane lane)
  492.     {
  493.         return this.infrastructureLaneChangeInfo.get(lane).getObject();
  494.     }

  496.     /** {@inheritDoc} */
  497.     @Override
  498.     public final SpeedLimitProspect getSpeedLimitProspect(final RelativeLane lane)
  499.     {
  500.         return this.speedLimitProspect.get(lane).getObject();
  501.     }

  503.     /** {@inheritDoc} */
  504.     @Override
  505.     public final Length getLegalLaneChangePossibility(final RelativeLane fromLane, final LateralDirectionality lat)
  506.     {
  507.         return this.legalLaneChangePossibility.get(fromLane).get(lat).getObject().getDistance(lat);
  508.     }

  510.     /** {@inheritDoc} */
  511.     @Override
  512.     public final Length getPhysicalLaneChangePossibility(final RelativeLane fromLane, final LateralDirectionality lat)
  513.     {
  514.         return this.physicalLaneChangePossibility.get(fromLane).get(lat).getObject().getDistance(lat);
  515.     }

  517.     /** {@inheritDoc} */
  518.     @Override
  519.     public final SortedSet<RelativeLane> getCrossSection()
  520.     {
  521.         return this.crossSection.getObject();
  522.     }

  524.     /**
  525.      * Returns time stamped infrastructure lane change info of a lane. A set is returned as multiple points may force lane
  526.      * changes. Which point is considered most critical is a matter of driver interpretation and may change over time. This is
  527.      * shown below. Suppose vehicle A needs to take the off-ramp, and that behavior is that the minimum distance per required
  528.      * lane change determines how critical it is. First, 400m before the lane-drop, the off-ramp is critical. 300m downstream,
  529.      * the lane-drop is critical. Info is sorted by distance, closest first.
  530.      * 
  531.      * <pre>
  532.      * _______
  533.      * _ _A_ _\_________
  534.      * _ _ _ _ _ _ _ _ _
  535.      * _________ _ _ ___
  536.      *          \_______
  537.      *     (-)        Lane-drop: 1 lane change  in 400m (400m per lane change)
  538.      *     (--------) Off-ramp:  3 lane changes in 900m (300m per lane change, critical)
  539.      *     
  540.      *     (-)        Lane-drop: 1 lane change  in 100m (100m per lane change, critical)
  541.      *     (--------) Off-ramp:  3 lane changes in 600m (200m per lane change)
  542.      * </pre>
  543.      * 
  544.      * @param lane RelativeLane; relative lateral lane
  545.      * @return time stamped infrastructure lane change info of a lane
  546.      */
  547.     public final TimeStampedObject<SortedSet<InfrastructureLaneChangeInfo>> getTimeStampedInfrastructureLaneChangeInfo(
  548.             final RelativeLane lane)
  549.     {
  550.         return this.infrastructureLaneChangeInfo.get(lane);
  551.     }

  553.     /**
  554.      * Returns the time stamped prospect for speed limits on a lane (dynamic speed limits may vary between lanes).
  555.      * @param lane RelativeLane; relative lateral lane
  556.      * @return time stamped prospect for speed limits on a lane
  557.      */
  558.     public final TimeStampedObject<SpeedLimitProspect> getTimeStampedSpeedLimitProspect(final RelativeLane lane)
  559.     {
  560.         return this.speedLimitProspect.get(lane);
  561.     }

  563.     /**
  564.      * Returns the time stamped distance over which a lane change remains legally possible.
  565.      * @param fromLane RelativeLane; lane from which the lane change possibility is requested
  566.      * @param lat LateralDirectionality; LEFT or RIGHT, null not allowed
  567.      * @return time stamped distance over which a lane change remains possible
  568.      * @throws NullPointerException if {@code lat == null}
  569.      */
  570.     public final TimeStampedObject<Length> getTimeStampedLegalLaneChangePossibility(final RelativeLane fromLane,
  571.             final LateralDirectionality lat)
  572.     {
  573.         TimeStampedObject<LaneChangePossibility> tsLcp = this.legalLaneChangePossibility.get(fromLane).get(lat);
  574.         LaneChangePossibility lcp = tsLcp.getObject();
  575.         return new TimeStampedObject<>(lcp.getDistance(lat), tsLcp.getTimestamp());
  576.     }

  578.     /**
  579.      * Returns the time stamped distance over which a lane change remains physically possible.
  580.      * @param fromLane RelativeLane; lane from which the lane change possibility is requested
  581.      * @param lat LateralDirectionality; LEFT or RIGHT, null not allowed
  582.      * @return time stamped distance over which a lane change remains possible
  583.      * @throws NullPointerException if {@code lat == null}
  584.      */
  585.     public final TimeStampedObject<Length> getTimeStampedPhysicalLaneChangePossibility(final RelativeLane fromLane,
  586.             final LateralDirectionality lat)
  587.     {
  588.         TimeStampedObject<LaneChangePossibility> tsLcp = this.physicalLaneChangePossibility.get(fromLane).get(lat);
  589.         LaneChangePossibility lcp = tsLcp.getObject();
  590.         return new TimeStampedObject<>(lcp.getDistance(lat), tsLcp.getTimestamp());
  591.     }

  593.     /**
  594.      * Returns a time stamped set of relative lanes representing the cross section. Lanes are sorted left to right.
  595.      * @return time stamped set of relative lanes representing the cross section
  596.      */
  597.     public final TimeStampedObject<SortedSet<RelativeLane>> getTimeStampedCrossSection()
  598.     {
  599.         return this.crossSection;
  600.     }

  602.     /** {@inheritDoc} */
  603.     @Override
  604.     public final String toString()
  605.     {
  606.         return "DirectInfrastructurePerception";
  607.     }

  609.     /**
  610.      * Helper class to return the distance over which a lane change is or is not possible. The distance is based on a
  611.      * LaneStructureRecord, and does not need an update as such.
  612.      * <p>
  613.      * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
  614.      * <br>
  615.      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
  616.      * <p>
  617.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 14 feb. 2018 <br>
  618.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  619.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  620.      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  621.      */
  622.     private class LaneChangePossibility
  623.     {

  625.         /** Structure the end of which determines the available distance. */
  626.         private final LaneStructureRecord record;

  628.         /** Relative distance towards nose or tail. */
  629.         private final double dx;

  631.         /** Whether to apply legal accessibility. */
  632.         private final boolean legal;

  634.         /**
  635.          * @param record LaneStructureRecord; structure the end of which determines the available distance
  636.          * @param dx Length; relative distance towards nose or tail
  637.          * @param legal boolean; whether to apply legal accessibility
  638.          */
  639.         LaneChangePossibility(final LaneStructureRecord record, final Length dx, final boolean legal)
  640.         {
  641.             this.record = record;
  642.             this.dx = dx.si;
  643.             this.legal = legal;
  644.         }

  646.         /**
  647.          * Returns the distance over which a lane change is (&gt;0) or is not (&lt;0) possible.
  648.          * @param lat LateralDirectionality; lateral direction
  649.          * @return Length distance over which a lane change is (&gt;0) or is not (&lt;0) possible
  650.          */
  651.         final Length getDistance(final LateralDirectionality lat)
  652.         {
  653.             double d = this.record.getStartDistance().si + this.record.getLane().getLength().si - this.dx;
  654.             if ((lat.isLeft() && this.record.possibleLeft(this.legal)) || (lat.isRight() && this.record.possibleRight(
  655.                 this.legal)))
  656.             {
  657.                 return Length.instantiateSI(d); // possible over d
  658.             }
  659.             return Length.instantiateSI(-d); // not possible over d
  660.         }

  662.     }

  664. }
Created with JaCoCo 0.8.8.202204050719