AbstractDelayedNeighborsPerception.java
package org.opentrafficsim.road.gtu.lane.perception.categories.neighbors;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;
import java.util.SortedSet;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Time;
import org.opentrafficsim.base.TimeStampedObject;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.base.parameters.ParameterTypeDuration;
import org.opentrafficsim.base.parameters.ParameterTypes;
import org.opentrafficsim.base.parameters.Parameters;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.perception.PerceptionException;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
import org.opentrafficsim.road.gtu.lane.perception.categories.AbstractDelayedPerceptionCategory;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
import nl.tudelft.simulation.dsol.SimRuntimeException;
/**
* <p>
* Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version 17 feb. 2017 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
public abstract class AbstractDelayedNeighborsPerception extends AbstractDelayedPerceptionCategory
implements NeighborsPerception
{
/** */
private static final long serialVersionUID = 20170217L;
/** Reaction time parameter type. */
protected static final ParameterTypeDuration TR = ParameterTypes.TR;
/** Time step parameter type. */
protected static final ParameterTypeDuration DT = ParameterTypes.DT;
/** First update time. */
private Time initialTime = null;
/** Wrapped direct perception. */
private final DirectNeighborsPerception direct;
/** Reaction time. */
private Duration reactionTime = null;
/** Time step of planner. */
private Duration plannerTimeStep = null;
/** Remainder between reaction time and planner time step. */
private Duration remainder = null;
/** Info type id base for first leaders. */
public static final String FIRSTLEADERS = "firstLeaders";
/** Info type id base for first followers. */
public static final String FIRSTFOLLOWERS = "firstFollower";
/** Info type id base for gtu alongside. */
public static final String GTUALONGSIDE = "gtuAlongside";
/** Info type id base for leaders. */
public static final String LEADERS = "leaders";
/** Info type id base for followers. */
public static final String FOLLOWERS = "followers";
/** Info type id for cross-section. */
public static final NeighborsInfoType<SortedSet<RelativeLane>> CROSSSECTION = new NeighborsInfoType<>("cross-section");
/** Id for odometer info type. */
public static final NeighborsInfoType<Length> ODOMETER = new NeighborsInfoType<>("odometer");
/** Override for left lane change. */
private boolean gtuAlongsideLeftOverride = false;
/** Override for right lane change. */
private boolean gtuAlongsideRightOverride = false;
/**
* Constructor.
* @param perception LanePerception; perception
*/
public AbstractDelayedNeighborsPerception(final LanePerception perception)
{
super(perception);
this.direct = new DirectNeighborsPerception(perception, HeadwayGtuType.COPY);
}
/** {@inheritDoc} */
@Override
public final void updateAll() throws GTUException, NetworkException, ParameterException
{
if (this.remainder == null)
{
try
{
// TODO The reaction time may differ between observed objects and vary over time
Parameters params = getPerception().getGtu().getParameters();
this.reactionTime = params.getParameter(TR);
this.plannerTimeStep = params.getParameter(DT);
double rem;
if (this.reactionTime.eq0())
{
rem = 0;
}
else if (this.reactionTime.gt(this.plannerTimeStep))
{
rem = this.reactionTime.si % this.plannerTimeStep.si;
}
else
{
rem = this.plannerTimeStep.si - this.reactionTime.si;
}
this.remainder = Duration.createSI(rem);
}
catch (ParameterException | GTUException exception)
{
throw new RuntimeException("Exception while setting up delayed neighors perception.", exception);
}
}
// direct perception in first few time steps; build up history
Time now = getPerception().getGtu().getSimulator().getSimulatorTime();
if (this.initialTime == null)
{
this.initialTime = now;
}
if (now.minus(this.initialTime).le(this.reactionTime))
{
updateAllDelayed();
return;
}
if (this.remainder.eq0())
{
// reaction time is multiple of time step, just do it now
updateAllDelayed();
}
else
{
// schedule actual update slightly in the future: this will be the snapshot for a future time step
Time scheduledTime = now.plus(this.remainder);
try
{
getPerception().getGtu().getSimulator().scheduleEventAbs(scheduledTime, this, this, "updateAllDelayed", null);
}
catch (SimRuntimeException exception)
{
throw new RuntimeException("Scheduling perception update in the past.", exception);
}
}
/*
* During the reaction time, an instantaneous lane change by a neighbor may be performed, which may cause an
* unreasonable lane change of the subject vehicle if it is not considered. Therefore, the 'gtuAlongSide' information is
* amended with a current snapshot of the surroundings. If the current first leaders/followers contains a GTU that the
* delayed leaders/followers do not contain, and that is within 50m, 'gtuAlongSide' is overruled with 'true', preventing
* a lane change.
*/
if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
{
this.direct.updateFirstFollowers(LateralDirectionality.LEFT);
this.direct.updateFirstLeaders(LateralDirectionality.LEFT);
this.direct.updateGtuAlongside(LateralDirectionality.LEFT);
this.gtuAlongsideLeftOverride = newFirstLeaderOrFollower(getFollowers(RelativeLane.LEFT),
this.direct.getFirstFollowers(LateralDirectionality.LEFT))
|| newFirstLeaderOrFollower(getLeaders(RelativeLane.LEFT),
this.direct.getFirstLeaders(LateralDirectionality.LEFT))
|| this.direct.isGtuAlongside(LateralDirectionality.LEFT);
}
if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
{
this.direct.updateFirstFollowers(LateralDirectionality.RIGHT);
this.direct.updateFirstLeaders(LateralDirectionality.RIGHT);
this.direct.updateGtuAlongside(LateralDirectionality.RIGHT);
this.gtuAlongsideRightOverride = newFirstLeaderOrFollower(getFollowers(RelativeLane.RIGHT),
this.direct.getFirstFollowers(LateralDirectionality.RIGHT))
|| newFirstLeaderOrFollower(getLeaders(RelativeLane.RIGHT),
this.direct.getFirstLeaders(LateralDirectionality.RIGHT))
|| this.direct.isGtuAlongside(LateralDirectionality.RIGHT);
}
}
/**
* Returns whether there is a gtu in the current set that is not present in the delayed set.
* @param delayedSet Iterable<? extends HeadwayGTU>; delayed set
* @param currentSet Set<? extends HeadwayGTU>; current set
* @return whether there is a gtu in the current set that is not present in the delayed set
*/
private boolean newFirstLeaderOrFollower(final Iterable<? extends HeadwayGTU> delayedSet,
final Set<? extends HeadwayGTU> currentSet)
{
Set<String> set = new HashSet<>();
for (HeadwayGTU gtu : delayedSet)
{
set.add(gtu.getId());
}
for (HeadwayGTU gtu : currentSet)
{
if (!set.contains(gtu.getId()) && gtu.getDistance().si < 50)
{
return true;
}
}
return false;
}
/**
* Returns whether to override the gtu alongside boolean as true.
* @param lat LateralDirectionality; lateral direction
* @return whether to override the gtu alongside boolean as true
*/
public final boolean isGtuAlongsideOverride(final LateralDirectionality lat)
{
return lat.isLeft() ? this.gtuAlongsideLeftOverride : this.gtuAlongsideRightOverride;
}
/**
* Performs actual update.
* @throws ParameterException if parameter is not present or is given a wrong value
* @throws NetworkException on error in the network
* @throws GTUException if not initialized
*/
// TODO private when DSOL allows
protected void updateAllDelayed() throws GTUException, NetworkException, ParameterException
{
try
{
getGtu().getReferencePosition();
}
catch (GTUException exception)
{
// GTU was destroyed
return;
}
this.direct.updateAll();
// below code is a copy of the updateAll() method in the direct perception TODO structure better
if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
{
updateFirstLeaders(LateralDirectionality.LEFT);
updateFirstFollowers(LateralDirectionality.LEFT);
updateGtuAlongside(LateralDirectionality.LEFT);
}
if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
{
updateFirstLeaders(LateralDirectionality.RIGHT);
updateFirstFollowers(LateralDirectionality.RIGHT);
updateGtuAlongside(LateralDirectionality.RIGHT);
}
for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
{
updateLeaders(lane);
updateFollowers(lane);
}
setInfo(CROSSSECTION,
new TimeStampedObject<>(getPerception().getLaneStructure().getExtendedCrossSection(), getTimestamp()));
setInfo(ODOMETER, new TimeStampedObject<>(getGtu().getOdometer(), getTimestamp()));
}
/** {@inheritDoc} */
@Override
public final void updateFirstLeaders(final LateralDirectionality lat)
throws ParameterException, GTUException, NetworkException
{
setInfo(NeighborsInfoType.getSortedSetType(FIRSTLEADERS), new RelativeLane(lat, 1),
this.direct.getTimeStampedFirstLeaders(lat));
}
/** {@inheritDoc} */
@Override
public final void updateFirstFollowers(final LateralDirectionality lat)
throws GTUException, ParameterException, NetworkException
{
setInfo(NeighborsInfoType.getSortedSetType(FIRSTFOLLOWERS), new RelativeLane(lat, 1),
this.direct.getTimeStampedFirstFollowers(lat));
}
/** {@inheritDoc} */
@Override
public final void updateGtuAlongside(final LateralDirectionality lat) throws GTUException, ParameterException
{
setInfo(NeighborsInfoType.getBooleanType(GTUALONGSIDE), new RelativeLane(lat, 1),
this.direct.isGtuAlongsideTimeStamped(lat));
}
/** {@inheritDoc} */
@Override
public final void updateLeaders(final RelativeLane lane) throws ParameterException, GTUException, NetworkException
{
setInfo(NeighborsInfoType.getIterableType(LEADERS), lane, this.direct.getTimeStampedLeaders(lane));
}
/** {@inheritDoc} */
@Override
public final void updateFollowers(final RelativeLane lane) throws GTUException, NetworkException, ParameterException
{
setInfo(NeighborsInfoType.getIterableType(FOLLOWERS), lane, this.direct.getTimeStampedFollowers(lane));
}
/**
* Returns the cross-section on which the most recent observed neighbors were determined.
* @return cross-section on which the most recent observed neighbors were determined
*/
public final SortedSet<RelativeLane> getDelayedCrossSection()
{
try
{
return getInfo(CROSSSECTION).getObject();
}
catch (PerceptionException exception)
{
throw new RuntimeException("Crosssection was not perceived.", exception);
}
}
/**
* Delayed information about the type of the neighbors. <br>
* @param <T> data type of info
*/
public static final class NeighborsInfoType<T> extends DelayedInfoType<T>
{
/** Map of id's and lane info types. */
private static final Map<String, NeighborsInfoType<?>> LANEINFOTYPES = new HashMap<>();
/**
* Construct new info.
* @param id String; id
*/
public NeighborsInfoType(final String id)
{
super(id, TR);
}
/**
* Returns a (cached) info type for a sorted set of GTU's.
* @param id String; id
* @return info type
*/
@SuppressWarnings("unchecked")
public static NeighborsInfoType<SortedSet<HeadwayGTU>> getSortedSetType(final String id)
{
if (!LANEINFOTYPES.containsKey(id))
{
LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
}
return (NeighborsInfoType<SortedSet<HeadwayGTU>>) LANEINFOTYPES.get(id);
}
/**
* Returns a (cached) info type for a sorted set of GTU's.
* @param id String; id
* @return info type
*/
@SuppressWarnings("unchecked")
public static NeighborsInfoType<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getIterableType(final String id)
{
if (!LANEINFOTYPES.containsKey(id))
{
LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
}
return (NeighborsInfoType<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>) LANEINFOTYPES.get(id);
}
/**
* Returns a (cached) info type for a sorted set of GTU's.
* @param id String; id
* @return info type
*/
@SuppressWarnings("unchecked")
public static NeighborsInfoType<Boolean> getBooleanType(final String id)
{
if (!LANEINFOTYPES.containsKey(id))
{
LANEINFOTYPES.put(id, new NeighborsInfoType<SortedSet<HeadwayGTU>>(id));
}
return (NeighborsInfoType<Boolean>) LANEINFOTYPES.get(id);
}
/** {@inheritDoc} */
@Override
public String toString()
{
return "NeighborsInfoType []";
}
}
}