DirectIntersectionPerception.java

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

  3. import java.util.ArrayList;
  4. import java.util.List;
  5. import java.util.SortedSet;

  7. import org.djunits.value.vdouble.scalar.Length;
  8. import org.djunits.value.vdouble.scalar.Speed;
  9. import org.djutils.exceptions.Try;
  10. import org.opentrafficsim.base.parameters.ParameterException;
  11. import org.opentrafficsim.base.parameters.ParameterTypeLength;
  12. import org.opentrafficsim.base.parameters.ParameterTypes;
  13. import org.opentrafficsim.core.geometry.OtsGeometryException;
  14. import org.opentrafficsim.core.geometry.OtsLine3d;
  15. import org.opentrafficsim.core.gtu.GtuException;
  16. import org.opentrafficsim.core.gtu.RelativePosition;
  17. import org.opentrafficsim.core.network.LateralDirectionality;
  18. import org.opentrafficsim.core.network.NetworkException;
  19. import org.opentrafficsim.core.network.route.Route;
  20. import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
  21. import org.opentrafficsim.road.gtu.lane.perception.LaneBasedObjectIterable;
  22. import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
  23. import org.opentrafficsim.road.gtu.lane.perception.LaneStructure.Entry;
  24. import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
  25. import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
  26. import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
  27. import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.HeadwayGtuType;
  28. import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
  29. import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGtu;
  30. import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayStopLine;
  31. import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
  32. import org.opentrafficsim.road.network.lane.CrossSectionLink;
  33. import org.opentrafficsim.road.network.lane.Lane;
  34. import org.opentrafficsim.road.network.lane.conflict.Conflict;
  35. import org.opentrafficsim.road.network.lane.conflict.Conflict.ConflictEnd;
  36. import org.opentrafficsim.road.network.lane.conflict.ConflictPriority;
  37. import org.opentrafficsim.road.network.lane.conflict.ConflictRule;
  38. import org.opentrafficsim.road.network.lane.conflict.ConflictType;
  39. import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
  40. import org.opentrafficsim.road.network.lane.object.trafficlight.TrafficLight;

  42. /**
  43.  * Perceives traffic lights and intersection conflicts.
  44.  * <p>
  45.  * Copyright (c) 2013-2023 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  46.  * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  47.  * </p>
  48.  * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
  49.  * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
  50.  * @author <a href="https://dittlab.tudelft.nl">Wouter Schakel</a>
  51.  */
  52. public class DirectIntersectionPerception extends LaneBasedAbstractPerceptionCategory implements IntersectionPerception
  53. {

  55.     /** */
  56.     private static final long serialVersionUID = 20160811L;

  58.     /** Look ahead parameter type. */
  59.     protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;

  61.     /** Margin to find upstream conflicts who's ends are downstream, needed as the algorithm searches beyond a location. */
  62.     private static final Length MARGIN = Length.instantiateSI(0.001);

  64.     /** Headway GTU type that should be used. */
  65.     private final HeadwayGtuType headwayGtuType;

  67.     /**
  68.      * @param perception LanePerception; perception
  69.      * @param headwayGtuType HeadwayGtuType; type of headway gtu to generate
  70.      */
  71.     public DirectIntersectionPerception(final LanePerception perception, final HeadwayGtuType headwayGtuType)
  72.     {
  73.         super(perception);
  74.         this.headwayGtuType = headwayGtuType;
  75.     }

  77.     /**
  78.      * Compute traffic lights.
  79.      * @param lane RelativeLane; lane
  80.      * @return PerceptionCollectable of traffic lights
  81.      */
  82.     private PerceptionCollectable<HeadwayTrafficLight, TrafficLight> computeTrafficLights(final RelativeLane lane)
  83.     {
  84.         try
  85.         {
  86.             Route route = getPerception().getGtu().getStrategicalPlanner().getRoute();
  87.             LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
  88.             Length pos = record.getStartDistance().neg();
  89.             pos = pos.plus(getGtu().getFront().getDx());
  90.             return new LaneBasedObjectIterable<HeadwayTrafficLight, TrafficLight>(getGtu(), TrafficLight.class, record,
  91.                     Length.max(Length.ZERO, pos), true, getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(),
  92.                     route)
  93.             {
  94.                 /** {@inheritDoc} */
  95.                 @Override
  96.                 public HeadwayTrafficLight perceive(final LaneBasedGtu perceivingGtu, final TrafficLight trafficLight,
  97.                         final Length distance)
  98.                 {
  99.                     try
  100.                     {
  101.                         return new HeadwayTrafficLight(trafficLight, distance);
  102.                     }
  103.                     catch (GtuException exception)
  104.                     {
  105.                         throw new RuntimeException(exception);
  106.                     }
  107.                 }
  108.             };
  109.         }
  110.         catch (GtuException | ParameterException exception)
  111.         {
  112.             throw new RuntimeException("Unexpected exception while computing traffic lights.", exception);
  113.         }

  115.     }

  117.     /**
  118.      * Compute conflicts.
  119.      * @param lane RelativeLane; lane
  120.      * @return PerceptionCollectable of conflicts
  121.      */
  122.     private PerceptionCollectable<HeadwayConflict, Conflict> computeConflicts(final RelativeLane lane)
  123.     {
  124.         try
  125.         {
  126.             Route route = getPerception().getGtu().getStrategicalPlanner().getRoute();
  127.             LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
  128.             Length pos = record.getStartDistance().neg().plus(getGtu().getRear().getDx());
  129.             while (pos.lt0() && !record.getPrev().isEmpty())
  130.             {
  131.                 pos = pos.plus(record.getLength());
  132.                 record = record.getPrev().get(0);
  133.             }
  134.             // find all ConflictEnd, and the most upstream relating position
  135.             List<LaneBasedObject> laneObjs;
  136.             if (record.isDownstreamBranch())
  137.             {
  138.                 laneObjs = record.getLane().getLaneBasedObjects(Length.max(Length.ZERO, pos), record.getLane().getLength());
  139.             }
  140.             else
  141.             {
  142.                 laneObjs = new ArrayList<>();
  143.             }
  144.             // TODO if conflicts span multiple lanes, this within-lane search fails
  145.             for (LaneBasedObject object : laneObjs)
  146.             {
  147.                 if (object instanceof ConflictEnd)
  148.                 {
  149.                     Conflict c = ((ConflictEnd) object).getConflict();
  150.                     Length cPos = c.getLongitudinalPosition().minus(MARGIN);
  151.                     pos = Length.min(pos, cPos);
  152.                 }
  153.             }
  154.             return new LaneBasedObjectIterable<HeadwayConflict, Conflict>(getGtu(), Conflict.class, record,
  155.                     Length.max(MARGIN.neg(), pos), true, getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(),
  156.                     route)
  157.             {
  158.                 /** {@inheritDoc} */
  159.                 @SuppressWarnings("synthetic-access")
  160.                 @Override
  161.                 public HeadwayConflict perceive(final LaneBasedGtu perceivingGtu, final Conflict conflict,
  162.                         final Length distance)
  163.                 {
  164.                     Conflict otherConflict = conflict.getOtherConflict();
  165.                     ConflictType conflictType = conflict.getConflictType();
  166.                     ConflictPriority conflictPriority = conflict.conflictPriority();
  167.                     Class<? extends ConflictRule> conflictRuleType = conflict.getConflictRule().getClass();
  168.                     String id = conflict.getId();
  169.                     Length length = conflict.getLength();
  170.                     Length conflictingLength = otherConflict.getLength();
  171.                     CrossSectionLink conflictingLink = otherConflict.getLane().getParentLink();

  173.                     // TODO get from link combination (needs to be a map property on the links)
  174.                     Length lookAhead =
  175.                             Try.assign(() -> getGtu().getParameters().getParameter(LOOKAHEAD), "Parameter not present.");
  176.                     Length conflictingVisibility = lookAhead;
  177.                     Speed conflictingSpeedLimit;
  178.                     try
  179.                     {
  180.                         conflictingSpeedLimit = otherConflict.getLane().getHighestSpeedLimit();
  181.                     }
  182.                     catch (NetworkException exception)
  183.                     {
  184.                         throw new RuntimeException("GTU type not available on conflicting lane.", exception);
  185.                     }

  187.                     // TODO limit 'conflictingVisibility' to first upstream traffic light, so GTU's behind it are ignored

  189.                     HeadwayConflict headwayConflict;
  190.                     try
  191.                     {
  192.                         PerceptionCollectable<HeadwayGtu, LaneBasedGtu> upstreamConflictingGTUs = otherConflict.getUpstreamGtus(
  193.                                 getGtu(), DirectIntersectionPerception.this.headwayGtuType, conflictingVisibility);
  194.                         PerceptionCollectable<HeadwayGtu, LaneBasedGtu> downstreamConflictingGTUs =
  195.                                 otherConflict.getDownstreamGtus(getGtu(), DirectIntersectionPerception.this.headwayGtuType,
  196.                                         conflictingVisibility);
  197.                         // TODO stop lines (current models happen not to use this, but should be possible)
  198.                         HeadwayStopLine stopLine = new HeadwayStopLine("stopLineId", Length.ZERO, conflict.getLane());
  199.                         HeadwayStopLine conflictingStopLine =
  200.                                 new HeadwayStopLine("conflictingStopLineId", Length.ZERO, conflict.getLane());

  202.                         Lane thisLane = conflict.getLane();
  203.                         Lane otherLane = otherConflict.getLane();
  204.                         Length pos1a = conflict.getLongitudinalPosition();
  205.                         Length pos2a = otherConflict.getLongitudinalPosition();
  206.                         Length pos1b = Length.min(pos1a.plus(conflict.getLength()), thisLane.getLength());
  207.                         Length pos2b = Length.min(pos2a.plus(otherConflict.getLength()), otherLane.getLength());
  208.                         OtsLine3d line1 = thisLane.getCenterLine();
  209.                         OtsLine3d line2 = otherLane.getCenterLine();
  210.                         double dStart = line1.getLocation(pos1a).distance(line2.getLocation(pos2a));
  211.                         double dEnd = line1.getLocation(pos1b).distance(line2.getLocation(pos2b));
  212.                         Length startWidth = Length
  213.                                 .instantiateSI(dStart + .5 * thisLane.getWidth(pos1a).si + .5 * otherLane.getWidth(pos2a).si);
  214.                         Length endWidth = Length
  215.                                 .instantiateSI(dEnd + .5 * thisLane.getWidth(pos1b).si + .5 * otherLane.getWidth(pos2b).si);

  217.                         headwayConflict = new HeadwayConflict(conflictType, conflictPriority, conflictRuleType, id, distance,
  218.                                 length, conflictingLength, upstreamConflictingGTUs, downstreamConflictingGTUs,
  219.                                 conflictingVisibility, conflictingSpeedLimit, conflictingLink,
  220.                                 HeadwayConflict.Width.linear(startWidth, endWidth), stopLine, conflictingStopLine, thisLane);

  222.                         Length trafficLightDistance = conflict.getOtherConflict()
  223.                                 .getTrafficLightDistance(perceivingGtu.getParameters().getParameter(ParameterTypes.LOOKAHEAD));
  224.                         if (trafficLightDistance != null && trafficLightDistance.le(lookAhead))
  225.                         {
  226.                             headwayConflict.setConflictingTrafficLight(trafficLightDistance, conflict.isPermitted());
  227.                         }
  228.                     }
  229.                     catch (GtuException | OtsGeometryException | ParameterException exception)
  230.                     {
  231.                         throw new RuntimeException("Could not create headway objects.", exception);
  232.                     }
  233.                     return headwayConflict;
  234.                 }
  235.             };
  236.         }
  237.         catch (GtuException | ParameterException exception)
  238.         {
  239.             throw new RuntimeException("Unexpected exception while computing conflicts.", exception);
  240.         }
  241.     }

  243.     /**
  244.      * Compute whether there is a conflict alongside.
  245.      * @param lat LateralDirectionality; lateral directionality
  246.      * @return boolean; whether there is a conflict alongside
  247.      */
  248.     private boolean computeConflictAlongside(final LateralDirectionality lat)
  249.     {
  250.         try
  251.         {
  252.             RelativeLane lane = new RelativeLane(lat, 1);
  253.             if (getPerception().getLaneStructure().getExtendedCrossSection().contains(lane))
  254.             {
  255.                 SortedSet<Entry<Conflict>> conflictEntries = getPerception().getLaneStructure().getUpstreamObjects(lane,
  256.                         Conflict.class, getGtu(), RelativePosition.FRONT);
  257.                 if (!conflictEntries.isEmpty())
  258.                 {
  259.                     Entry<Conflict> entry = conflictEntries.first();
  260.                     return entry.getDistance().si < entry.getLaneBasedObject().getLength().si + getGtu().getLength().si;
  261.                 }
  262.             }
  263.             return false;
  264.         }
  265.         catch (GtuException | ParameterException exception)
  266.         {
  267.             throw new RuntimeException("Unexpected exception while computing conflict alongside.", exception);
  268.         }
  269.     }

  271.     /** {@inheritDoc} */
  272.     @Override
  273.     public final PerceptionCollectable<HeadwayTrafficLight, TrafficLight> getTrafficLights(final RelativeLane lane)
  274.     {
  275.         return computeIfAbsent("trafficLights", () -> computeTrafficLights(lane), lane);
  276.     }

  278.     /** {@inheritDoc} */
  279.     @Override
  280.     public final PerceptionCollectable<HeadwayConflict, Conflict> getConflicts(final RelativeLane lane)
  281.     {
  282.         return computeIfAbsent("conflicts", () -> computeConflicts(lane), lane);
  283.     }

  285.     /** {@inheritDoc} */
  286.     @Override
  287.     public final boolean isAlongsideConflictLeft()
  288.     {
  289.         return computeIfAbsent("alongside", () -> computeConflictAlongside(LateralDirectionality.LEFT),
  290.                 LateralDirectionality.LEFT);
  291.     }

  293.     /** {@inheritDoc} */
  294.     @Override
  295.     public final boolean isAlongsideConflictRight()
  296.     {
  297.         return computeIfAbsent("alongside", () -> computeConflictAlongside(LateralDirectionality.RIGHT),
  298.                 LateralDirectionality.RIGHT);
  299.     }

  301.     /** {@inheritDoc} */
  302.     @Override
  303.     public void updateAll() throws GtuException, NetworkException, ParameterException
  304.     {
  305.         // lazy evaluation
  306.     }

  308.     /** {@inheritDoc} */
  309.     @Override
  310.     public final String toString()
  311.     {
  312.         return "DirectIntersectionPerception";
  313.     }

  315. }
Created with JaCoCo 0.8.8.202204050719