Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 257:9281878ff19e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
untested collision/crossing computation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 24 Jul 2012 03:27:29 -0400
parents dc1faa7287bd
children d90be3c02267
comparison
equal deleted inserted replaced
256:dc1faa7287bd 257:9281878ff19e
53 self.predictedPositions = {0: initialPosition} 53 self.predictedPositions = {0: initialPosition}
54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
55 55
56 def getControl(self): 56 def getControl(self):
57 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) 57 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
58
59
60 class ExtrapolationParameters:
61 def __init__(self, name):
62 self.name = name
63
64 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed):
65 '''extrapolationParameters specific to each method (in name field) '''
66 if extrapolationHypothesis.name == 'constant velocity':
67 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
68 else:
69 print('Unknown extrapolation hypothesis')
70 return []
71
72 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
73 '''returns the lists of collision points and crossing zones '''
74 collisionPoints = []
75 TTCs = []
76 crossingZones = []
77 pPETs = []
78 for et1 in extrapolatedTrajectories1:
79 for et2 in extrapolatedTrajectories2:
80
81 t = 1
82 p1 = et1.predictPosition(t)
83 p2 = et2.predictPosition(t)
84 while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold:
85 p1 = et1.predictPosition(t)
86 p2 = et2.predictPosition(t)
87 t += 1
88
89 if t <= timeHorizon:
90 TTCs.append(t) # todo store probability
91 collisionPoints.append((p1+p2).multiply(0.5))
92 else: # check if there is a crossing zone
93 for t1 in xrange(timeHorizon-1):
94 for t2 in xrange(timeHorizon-1):
95 cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
96 if cz:
97 crossingZones.append(cz)
98 pPETs.append(abs(t1-t2))
99 break
100 return collisionPoints, crossingZones, TTCs, pPETs
101
58 102
59 103
60 # Default values: to remove because we cannot tweak that from a script where the value may be different 104 # Default values: to remove because we cannot tweak that from a script where the value may be different
61 FPS= 25 # No. of frame per second (FPS) 105 FPS= 25 # No. of frame per second (FPS)
62 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec 106 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec