Mercurial Hosting > traffic-intelligence
changeset 260:36cb40c51a5e
modified the organization of the code
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 24 Jul 2012 18:07:23 -0400 |
parents | 8ab76b95ee72 |
children | 4aa792cb0fa9 |
files | python/extrapolation.py |
diffstat | 1 files changed, 52 insertions(+), 5 deletions(-) [+] |
line wrap: on
line diff
--- a/python/extrapolation.py Tue Jul 24 15:18:12 2012 -0400 +++ b/python/extrapolation.py Tue Jul 24 18:07:23 2012 -0400 @@ -34,7 +34,6 @@ class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): '''Extrapolated trajectory at constant speed or acceleration - TODO add limits if acceleration TODO generalize by passing a series of velocities/accelerations''' def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None): @@ -68,10 +67,19 @@ def __init__(self, name): self.name = name -def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): +def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity): '''extrapolationParameters specific to each method (in name field) ''' if extrapolationParameters.name == 'constant velocity': - return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] + return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, 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, + extrapolationParameters.accelerationDistribution, + extrapolationParameters.steeringDistribution, + maxSpeed = extrapolationParameters.maxSpeed)) + return extrapolatedTrajectories else: print('Unknown extrapolation hypothesis') return [] @@ -85,17 +93,20 @@ self.probability = probability self.indicator = indicator +def saveSafetyPoints(out, objNum1, objNum2, instant, points): + for p in points: + out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator)) + def computeExpectedIndicator(points): from numpy import sum return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) -def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): +def computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): '''returns the lists of collision points and crossing zones ''' collisionPoints = [] crossingZones = [] for et1 in extrapolatedTrajectories1: for et2 in extrapolatedTrajectories2: - t = 1 p1 = et1.predictPosition(t) p2 = et2.predictPosition(t) @@ -124,7 +135,43 @@ t1 += 1 return collisionPoints, crossingZones +def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False): + collisionPoints={} + crossingZones={} + #TTCs = {} + #pPETs = {} + 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)) + collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) + # if len(collisionPoints[i]) > 0: + # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) + # if len(crossingZones[i]) > 0: + # pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i]) + saveSafetyPoints(outCP, obj1.num, obj2.num, i, collisionPoints[i]) + saveSafetyPoints(outCZ, obj1.num, obj2.num, i, crossingZones[i]) + + if debug: + from matplotlib.pyplot import figure, axis + figure() + obj1.draw('r') + obj2.draw('b') + for et in extrapolatedTrajectories1: + et.predictPosition(timeHorizon) + et.draw('rx') + for et in extrapolatedTrajectories2: + et.predictPosition(timeHorizon) + et.draw('bx') + axis('equal') + + + # probability of successful evasive action = 1-P(Collision) using larger control values + + return collisionPoints, crossingZones + +############### # Default values: to remove because we cannot tweak that from a script where the value may be different FPS= 25 # No. of frame per second (FPS)