Commit 8232212d authored by Benedikt Kleinmeier's avatar Benedikt Kleinmeier
Browse files

Started implementation of "bhm/NavigationEvasion.java" and unit tests and added two test scenarios

parent 039848b1
Pipeline #347489 passed with stages
in 151 minutes and 41 seconds
{
"name" : "BHM-Defaults",
"description" : "",
"release" : "1.14",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.EventtimePedestrianIdOutputFile",
"filename" : "postvis.traj",
"processors" : [ 1, 2 ]
}, {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOverlapOutputFile",
"filename" : "overlaps.csv",
"processors" : [ 3 ]
}, {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.NoDataKeyOutputFile",
"filename" : "overlapCount.txt",
"processors" : [ 4 ]
} ],
"processors" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.FootStepProcessor",
"id" : 1
}, {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.FootStepTargetIDProcessor",
"id" : 2
}, {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.PedestrianOverlapProcessor",
"id" : 3
}, {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.NumberOverlapsProcessor",
"id" : 4,
"attributesType" : "org.vadere.state.attributes.processor.AttributesNumberOverlapsProcessor",
"attributes" : {
"pedestrianOverlapProcessorId" : 3
}
} ],
"isTimestamped" : true,
"isWriteMetaData" : false
},
"scenario" : {
"mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel",
"attributesModel" : {
"org.vadere.state.attributes.models.AttributesBHM" : {
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036,
"stepLengthDeviation" : false,
"navigationCluster" : false,
"navigationFollower" : false,
"directionWallDistance" : false,
"tangentialEvasion" : true,
"sidewaysEvasion" : false,
"onlyEvadeContraFlow" : false,
"makeSmallSteps" : false,
"followerProximityNavigation" : true,
"differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ],
"varyingBehaviour" : true,
"adaptiveBehaviourDensity" : false,
"adaptiveBehaviourStepsRemained" : [ ],
"switchBehaviour" : false,
"evasionDetourThreshold" : 0.1,
"onlyEvadeContraFlowAngle" : 2.0943951023931953,
"followerAngleMovement" : 1.5707963267948966,
"followerAnglePosition" : 1.5707963267948966,
"followerDistance" : 10.0,
"smallStepResolution" : 5,
"plannedStepsAhead" : 5,
"obstacleRepulsionReach" : 1.0,
"obstacleRepulsionMaxWeight" : 0.5,
"distanceToKeep" : 0.5,
"backwardsAngle" : 1.5707963267948966,
"reconsiderOldTargets" : false,
"targetThresholdX" : 1.7976931348623157E308,
"targetThresholdY" : 1.7976931348623157E308,
"spaceToKeep" : 0.01,
"stepAwayFromCollisions" : false
}
},
"attributesSimulation" : {
"finishTime" : 200.0,
"simTimeStepLength" : 0.4,
"realTimeSimTimeRatio" : 0.1,
"writeSimulationData" : true,
"visualizationEnabled" : true,
"printFPS" : false,
"digitsPerCoordinate" : 2,
"useFixedSeed" : true,
"fixedSeed" : -8539920412942853482,
"simulationSeed" : 0
},
"attributesPsychology" : {
"usePsychologyLayer" : false,
"psychologyLayer" : {
"perception" : "SimplePerceptionModel",
"cognition" : "SimpleCognitionModel"
}
},
"attributesStrategy" : {
"useStrategyModel" : false,
"strategyModel" : null,
"arguments" : [ ],
"requiredDataProcessorIds" : [ ]
},
"topography" : {
"attributes" : {
"bounds" : {
"x" : 0.0,
"y" : 0.0,
"width" : 20.0,
"height" : 2.0
},
"boundingBoxWidth" : 0.5,
"bounded" : true,
"referenceCoordinateSystem" : null
},
"obstacles" : [ ],
"measurementAreas" : [ ],
"stairs" : [ ],
"targets" : [ {
"id" : 1,
"absorbing" : true,
"shape" : {
"x" : 18.5,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"waitingTime" : 0.0,
"waitingTimeYellowPhase" : 0.0,
"parallelWaiters" : 0,
"individualWaiting" : true,
"deletionDistance" : 0.1,
"startingWithRedLight" : false,
"nextSpeed" : -1.0
}, {
"id" : 2,
"absorbing" : true,
"shape" : {
"x" : 0.5,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"waitingTime" : 0.0,
"waitingTimeYellowPhase" : 0.0,
"parallelWaiters" : 0,
"individualWaiting" : true,
"deletionDistance" : 0.1,
"startingWithRedLight" : false,
"nextSpeed" : -1.0
} ],
"targetChangers" : [ ],
"absorbingAreas" : [ ],
"sources" : [ {
"id" : 3,
"shape" : {
"x" : 1.6,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution",
"distributionParameters" : [ 1.0 ],
"spawnNumber" : 20,
"maxSpawnNumberTotal" : -1,
"startTime" : 0.0,
"endTime" : 0.0,
"spawnAtRandomPositions" : false,
"spawnAtGridPositionsCA" : false,
"useFreeSpaceOnly" : true,
"targetIds" : [ 1 ],
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN",
"attributesPedestrian" : null
}, {
"id" : 4,
"shape" : {
"x" : 17.4,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution",
"distributionParameters" : [ 1.0 ],
"spawnNumber" : 20,
"maxSpawnNumberTotal" : -1,
"startTime" : 0.0,
"endTime" : 0.0,
"spawnAtRandomPositions" : false,
"spawnAtGridPositionsCA" : false,
"useFreeSpaceOnly" : true,
"targetIds" : [ 2 ],
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN",
"attributesPedestrian" : null
} ],
"dynamicElements" : [ ],
"attributesPedestrian" : {
"radius" : 0.2,
"densityDependentSpeed" : false,
"speedDistributionMean" : 1.34,
"speedDistributionStandardDeviation" : 0.26,
"minimumSpeed" : 0.5,
"maximumSpeed" : 2.2,
"acceleration" : 2.0,
"footstepHistorySize" : 4,
"searchRadius" : 4.0,
"walkingDirectionCalculation" : "BY_TARGET_CENTER",
"walkingDirectionSameIfAngleLessOrEqual" : 45.0
},
"teleporter" : null,
"attributesCar" : null
},
"stimulusInfos" : [ ]
}
}
\ No newline at end of file
{
"name" : "CounterFlow-CounterflowCognitionModel",
"description" : "",
"release" : "1.14",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.EventtimePedestrianIdOutputFile",
"filename" : "postvis.traj",
"processors" : [ 2, 3 ]
} ],
"processors" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.FootStepProcessor",
"id" : 2
}, {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.FootStepTargetIDProcessor",
"id" : 3
} ],
"isTimestamped" : true,
"isWriteMetaData" : false
},
"scenario" : {
"mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel",
"attributesModel" : {
"org.vadere.state.attributes.models.AttributesBHM" : {
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036,
"stepLengthDeviation" : true,
"navigationCluster" : false,
"navigationFollower" : false,
"directionWallDistance" : false,
"tangentialEvasion" : false,
"sidewaysEvasion" : false,
"onlyEvadeContraFlow" : false,
"makeSmallSteps" : false,
"followerProximityNavigation" : false,
"differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ],
"varyingBehaviour" : false,
"adaptiveBehaviourDensity" : false,
"adaptiveBehaviourStepsRemained" : [ ],
"switchBehaviour" : false,
"evasionDetourThreshold" : 0.1,
"onlyEvadeContraFlowAngle" : 2.0943951023931953,
"followerAngleMovement" : 1.5707963267948966,
"followerAnglePosition" : 1.5707963267948966,
"followerDistance" : 10.0,
"smallStepResolution" : 5,
"plannedStepsAhead" : 5,
"obstacleRepulsionReach" : 0.5,
"obstacleRepulsionMaxWeight" : 6.0,
"distanceToKeep" : 0.5,
"backwardsAngle" : 1.5707963267948966,
"reconsiderOldTargets" : false,
"targetThresholdX" : 1.7976931348623157E308,
"targetThresholdY" : 1.7976931348623157E308,
"spaceToKeep" : 0.01,
"stepAwayFromCollisions" : false
},
"org.vadere.state.attributes.models.AttributesFloorField" : {
"createMethod" : "HIGH_ACCURACY_FAST_MARCHING",
"potentialFieldResolution" : 0.1,
"obstacleGridPenalty" : 0.1,
"targetAttractionStrength" : 1.0,
"cacheType" : "NO_CACHE",
"cacheDir" : "",
"timeCostAttributes" : {
"standardDeviation" : 0.2,
"type" : "DISTANCE_TO_OBSTACLES",
"obstacleDensityWeight" : 1.0,
"pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5,
"pedestrianWeight" : 3.5,
"queueWidthLoading" : 1.0,
"pedestrianDynamicWeight" : 6.0,
"loadingType" : "CONSTANT",
"width" : 0.2,
"height" : 1.0
}
}
},
"attributesSimulation" : {
"finishTime" : 200.0,
"simTimeStepLength" : 0.4,
"realTimeSimTimeRatio" : 0.0,
"writeSimulationData" : true,
"visualizationEnabled" : true,
"printFPS" : false,
"digitsPerCoordinate" : 2,
"useFixedSeed" : true,
"fixedSeed" : 1,
"simulationSeed" : 1
},
"attributesPsychology" : {
"usePsychologyLayer" : false,
"psychologyLayer" : {
"perception" : "SimplePerceptionModel",
"cognition" : "CooperativeCognitionModel"
}
},
"attributesStrategy" : {
"useStrategyModel" : false,
"strategyModel" : null,
"arguments" : [ ],
"requiredDataProcessorIds" : [ ]
},
"topography" : {
"attributes" : {
"bounds" : {
"x" : 0.0,
"y" : 0.0,
"width" : 20.0,
"height" : 2.0
},
"boundingBoxWidth" : 0.5,
"bounded" : true,
"referenceCoordinateSystem" : null
},
"obstacles" : [ ],
"measurementAreas" : [ ],
"stairs" : [ ],
"targets" : [ {
"id" : 1,
"absorbing" : true,
"shape" : {
"x" : 18.5,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"waitingTime" : 0.0,
"waitingTimeYellowPhase" : 0.0,
"parallelWaiters" : 0,
"individualWaiting" : true,
"deletionDistance" : 0.1,
"startingWithRedLight" : false,
"nextSpeed" : -1.0
}, {
"id" : 2,
"absorbing" : true,
"shape" : {
"x" : 0.5,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"waitingTime" : 0.0,
"waitingTimeYellowPhase" : 0.0,
"parallelWaiters" : 0,
"individualWaiting" : true,
"deletionDistance" : 0.1,
"startingWithRedLight" : false,
"nextSpeed" : -1.0
} ],
"targetChangers" : [ ],
"absorbingAreas" : [ ],
"sources" : [ {
"id" : 3,
"shape" : {
"x" : 1.6,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution",
"distributionParameters" : [ 1.0 ],
"spawnNumber" : 20,
"maxSpawnNumberTotal" : -1,
"startTime" : 0.0,
"endTime" : 0.0,
"spawnAtRandomPositions" : false,
"spawnAtGridPositionsCA" : false,
"useFreeSpaceOnly" : true,
"targetIds" : [ 1 ],
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN",
"attributesPedestrian" : null
}, {
"id" : 4,
"shape" : {
"x" : 17.4,
"y" : 0.5,
"width" : 1.0,
"height" : 1.0,
"type" : "RECTANGLE"
},
"interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution",
"distributionParameters" : [ 1.0 ],
"spawnNumber" : 20,
"maxSpawnNumberTotal" : -1,
"startTime" : 0.0,
"endTime" : 0.0,
"spawnAtRandomPositions" : false,
"spawnAtGridPositionsCA" : false,
"useFreeSpaceOnly" : true,
"targetIds" : [ 2 ],
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN",
"attributesPedestrian" : null
} ],
"dynamicElements" : [ ],
"attributesPedestrian" : {
"radius" : 0.2,
"densityDependentSpeed" : false,
"speedDistributionMean" : 1.34,
"speedDistributionStandardDeviation" : 0.26,
"minimumSpeed" : 0.5,
"maximumSpeed" : 2.2,
"acceleration" : 2.0,
"footstepHistorySize" : 4,
"searchRadius" : 4.0,
"walkingDirectionCalculation" : "BY_TARGET_CENTER",
"walkingDirectionSameIfAngleLessOrEqual" : 45.0
},
"teleporter" : null,
"attributesCar" : null
},
"stimulusInfos" : [ ]
}
}
\ No newline at end of file
package org.vadere.simulator.models.bhm;
import org.apache.commons.lang3.tuple.ImmutablePair;
import org.apache.commons.lang3.tuple.Pair;
import org.vadere.state.attributes.models.AttributesBHM;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.logging.Logger;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
/**
* Let agents evade to the right-hand side. Use following evasion preference:
*
* <ol>
* <li>Evade tangentially (45°) with current speed).</li>
* <li>Evade tangentially (45°) with speed/2) if not enough space in previous step.</li>
* <li>Evade sideways (90°) with speed/2) if not enough space in previous step.</li>
* <li>Evade sideways (90°) with speed/4) if not enough space in previous step.</li>
* <li>Wait.</li>
* </ol>
*/
public class NavigationEvasion implements Navigation {
private static Logger logger = Logger.getLogger(NavigationEvasion.class);
private final PedestrianBHM me;
private final Topography topography;
private final AttributesBHM attributesBHM;
public NavigationEvasion(PedestrianBHM me, Topography topography) {
this.me = me;
this.topography = topography;
this.attributesBHM = me.getAttributesBHM();
}
@Override
public VPoint getNavigationPosition() {
VPoint nextPosition = me.getPosition();
VPoint currentWalkingDirection = getCurrentTargetDirectionNormedByGradient();
double stepLength = me.getStepLength();
List<Pedestrian> closePedestrians = topography.getSpatialMap(Pedestrian.class)
.getObjects(me.getPosition(), me.getAttributes().getSearchRadius())
.stream()
.filter(pedestrian -> pedestrian.getId() != me.getId())
.collect(Collectors.toList());
List<Pair<Double, Double>> rotationAndSlowDownStrategies = createRotationAndSlowDownStrategies();
for (Pair<Double, Double> strategy : rotationAndSlowDownStrategies) {
Double rotation = strategy.getLeft();
Double slowDownFactor = strategy.getRight();
double stepLengthTrial = stepLength * slowDownFactor;
VPoint possiblePosition = getNextPosition(currentWalkingDirection, rotation, stepLengthTrial);
if (collideWithPedestrian(possiblePosition, me.getRadius(), closePedestrians) == false) {
if (me.collidesWithObstacle(possiblePosition) == false) {
nextPosition = possiblePosition;
break;
}
}
}
return nextPosition;
}
public VPoint getCurrentTargetDirectionNormedByGradient() {
Vector2D targetGradient = me.getPotentialFieldTarget().getTargetPotentialGradient(me.getPosition(), me);
Vector2D pedestrianWalkingDirection = targetGradient.rotate(Math.toRadians(180));
return pedestrianWalkingDirection.norm();
}
public VPoint getNextPosition(VPoint currentWalkingDirection, double rotationAngleDegCcw, double stepLength) {
double rotationAngleRad = Math.toRadians(rotationAngleDegCcw);
VPoint evasionDirection = currentWalkingDirection.rotate(rotationAngleRad);
VPoint nextPosition = me.getPosition().add(evasionDirection.scalarMultiply(stepLength));
return nextPosition;
}
private List<Pair<Double, Double>> createRotationAndSlowDownStrategies() {
// 1st Double = rotation, 2nd Double = slow down from current speed
List<Pair<Double, Double>> rotationAndSlowDownStrategies = new ArrayList<>();
rotationAndSlowDownStrategies.add(new ImmutablePair<Double, Double>(-45.0, 1.0));
rotationAndSlowDownStrategies.add(new ImmutablePair<Double, Double>(-45.0, 0.5));
rotationAndSlowDownStrategies.add(new ImmutablePair<Double, Double>(-90.0, 0.5));
rotationAndSlowDownStrategies.add(new ImmutablePair<Double, Double>(-90.0, 0.25));
return rotationAndSlowDownStrategies;
}
private boolean collideWithPedestrian(VPoint position, double allowedDelta, List<Pedestrian> closePedestrians) {
boolean isCollision = false;
for (Pedestrian other : closePedestrians) {
if (position.distance(other.getPosition()) < allowedDelta) {
isCollision = true;
break;
}
}
return isCollision;
}
}
......@@ -114,6 +114,8 @@ public class PedestrianBHM extends Pedestrian {
return potentialFieldTarget != null;
}
public IPotentialFieldTarget getPotentialFieldTarget() { return potentialFieldTarget; }
private void setEvasionStrategy() {
if (attributesBHM.isSwitchBehaviour()) {
......
package org.vadere.simulator.models.bhm;
import org.junit.Test;
import org.vadere.simulator.models.potential.fields.IPotentialFieldTargetGrid;
import org.vadere.simulator.projects.Domain;
import org.vadere.state.attributes.Attributes;
import org.vadere.state.attributes.models.AttributesBHM;
import org.vadere.state.attributes.models.AttributesFloorField;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.attributes.scenario.AttributesTarget;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VCircle;
import org.vadere.util.geometry.shapes.VPoint;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import static org.junit.Assert.assertEquals;
public class NavigationEvasionTest {
// Static Variables
public static double ALLOWED_DOUBLE_ERROR = 10e-2;
// Member Variables
private Topography topography;
private PedestrianBHM pedestrian1;