Mercurial Hosting > traffic-intelligence
changeset 264:a04a6af4b810
modified functions to generate extrapolated trajectories for different positions/velocities
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 26 Jul 2012 03:54:41 -0400 |
parents | c71540470057 |
children | 7a3bf04cf016 |
files | python/extrapolation.py |
diffstat | 1 files changed, 22 insertions(+), 9 deletions(-) [+] |
line wrap: on
line diff
diff -r c71540470057 -r a04a6af4b810 python/extrapolation.py --- a/python/extrapolation.py Wed Jul 25 22:06:51 2012 -0400 +++ b/python/extrapolation.py Thu Jul 26 03:54:41 2012 -0400 @@ -62,24 +62,33 @@ def getControl(self): return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) - class ExtrapolationParameters: def __init__(self, name): self.name = name -def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity): +def createExtrapolatedTrajectories(extrapolationParameters, obj, instant): '''extrapolationParameters specific to each method (in name field) ''' if extrapolationParameters.name == 'constant velocity': - return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = extrapolationParameters.maxSpeed)] + return [ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), + maxSpeed = extrapolationParameters.maxSpeed)] elif extrapolationParameters.name == 'normal adaptation': - # generate several and with the right distribution extrapolatedTrajectories = [] for i in xrange(extrapolationParameters.nExtrapolatedTrajectories): - extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(initialPosition, initialVelocity, + extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(obj.getPositionAtInstant(instant), + obj.getVelocityAtInstant(instant), extrapolationParameters.accelerationDistribution, extrapolationParameters.steeringDistribution, maxSpeed = extrapolationParameters.maxSpeed)) return extrapolatedTrajectories + elif extrapolationParameters.name == 'pointset': + extrapolatedTrajectories = [] + features = [f for f in obj.features if f.existsAtInstant(instant)] + positions = [f.getPositionAtInstant(instant) for f in features] + velocities = [f.getVelocityAtInstant(instant) for f in features] + for initialPosition,initialVelocity in zip(positions, velocities): + extrapolatedTrajectories.append(ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, + maxSpeed = extrapolationParameters.maxSpeed)) + return extrapolatedTrajectories else: print('Unknown extrapolation hypothesis') return [] @@ -135,15 +144,19 @@ t1 += 1 return collisionPoints, crossingZones -def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False): +def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None): collisionPoints={} crossingZones={} #TTCs = {} #pPETs = {} - commonTimeInterval = obj1.commonTimeInterval(obj2) + if timeInterval: + commonTimeInterval = timeInterval + else: + commonTimeInterval = obj1.commonTimeInterval(obj2) for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors - extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1.getPositionAtInstant(i), obj1.getVelocityAtInstant(i)) - extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2.getPositionAtInstant(i), obj2.getVelocityAtInstant(i)) + print(obj1.num, obj2.num, i) + extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1, i) + extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2, i) collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) # if len(collisionPoints[i]) > 0: