Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 300:f65b828e5521

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
working on trajectory simulation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 13 Feb 2013 18:26:49 -0500
parents 586ead03fc00
children 124f85c6cfae 414b2e7cd873
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
1 #! /usr/bin/env python
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
2 '''Library for motion prediction methods'''
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
3
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
4 import moving
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
5 import math
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
6 import random
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
7
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
8 class PredictedTrajectory:
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
9 '''Class for predicted trajectories with lazy evaluation
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
10 if the predicted position has not been already computed, compute it
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
11
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
12 it should also have a probability'''
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
13
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
14 def __init__(self):
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
15 self.probability = 0.
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
16 self.predictedPositions = {}
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
17 self.predictedSpeedOrientations = {}
300
f65b828e5521 working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 296
diff changeset
18 #self.collisionPoints = {}
f65b828e5521 working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 296
diff changeset
19 #self.crossingZones = {}
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
20
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
21 def predictPosition(self, nTimeSteps):
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
22 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
23 self.predictPosition(nTimeSteps-1)
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
24 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
25 return self.predictedPositions[nTimeSteps]
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
26
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
27 def getPredictedTrajectory(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
28 return moving.Trajectory.fromPointList(self.predictedPositions.values())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
29
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
30 def getPredictedSpeeds(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
31 return [so.norm for so in self.predictedSpeedOrientations.values()]
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
32
270
05c9b0cb8202 updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 269
diff changeset
33 def draw(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
05c9b0cb8202 updates for drawing
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 269
diff changeset
34 self.getPredictedTrajectory().draw(options, withOrigin, timeStep, **kwargs)
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
35
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
36 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
37 '''Predicted trajectory at constant speed or acceleration
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
38 TODO generalize by passing a series of velocities/accelerations'''
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
39
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
40 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None):
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
41 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
42 self.maxSpeed = maxSpeed
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
43 self.probability = probability
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
44 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
45 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
46
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
47 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
48 return self.control
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
49
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
50 class PredictedTrajectoryNormalAdaptation(PredictedTrajectory):
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
51 '''Random small adaptation of vehicle control '''
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
52 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1, maxSpeed = None):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
53 '''Constructor
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
54 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
55 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
56 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
57 self.steeringDistribution = steeringDistribution
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
58 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
59 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
60 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
61 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
62
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
63 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
64 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
65
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
66 class PredictionParameters:
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
67 def __init__(self, name, maxSpeed):
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
68 self.name = name
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
69 self.maxSpeed = maxSpeed
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
70
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
71 def __str__(self):
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
72 return '{0} {1}'.format(self.name, self.maxSpeed)
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
73
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
74 class ConstantPredictionParameters(PredictionParameters):
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
75 def __init__(self, maxSpeed):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
76 PredictionParameters.__init__(self, 'constant velocity', maxSpeed)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
77
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
78 def generatePredictedTrajectories(self, obj, instant):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
79 return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant),
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
80 maxSpeed = self.maxSpeed)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
81
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
82 class NormalAdaptationPredictionParameters(PredictionParameters):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
83 def __init__(self, maxSpeed, nPredictedTrajectories, maxAcceleration, maxSteering):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
84 PredictionParameters.__init__(self, 'normal adaptation', maxSpeed)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
85 self.nPredictedTrajectories = nPredictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
86 self.maxAcceleration = maxAcceleration
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
87 self.maxSteering = maxSteering
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
88 self.accelerationDistribution = lambda: random.triangular(-self.maxAcceleration,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
89 self.maxAcceleration, 0.)
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
90 self.steeringDistribution = lambda: random.triangular(-self.maxSteering,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
91 self.maxSteering, 0.)
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
92
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
93 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
94 return PredictionParameters.__str__(self)+' {0} {1} {2}'.format(self.nPredictedTrajectories,
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
95 self.maxAcceleration,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
96 self.maxSteering)
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
97
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
98 def generatePredictedTrajectories(self, obj, instant):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
99 predictedTrajectories = []
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
100 for i in xrange(self.nPredictedTrajectories):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
101 predictedTrajectories.append(PredictedTrajectoryNormalAdaptation(obj.getPositionAtInstant(instant),
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
102 obj.getVelocityAtInstant(instant),
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
103 self.accelerationDistribution,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
104 self.steeringDistribution,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
105 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
106 return predictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
107
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
108 class PointSetPredictionParameters(PredictionParameters):
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
109 # todo generate several trajectories with normal adaptatoins from each position (feature)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
110 def __init__(self, maxSpeed):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
111 PredictionParameters.__init__(self, 'point set', maxSpeed)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
112
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
113 def generatePredictedTrajectories(self, obj, instant):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
114 predictedTrajectories = []
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
115 features = [f for f in obj.features if f.existsAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
116 positions = [f.getPositionAtInstant(instant) for f in features]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
117 velocities = [f.getVelocityAtInstant(instant) for f in features]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
118 for initialPosition,initialVelocity in zip(positions, velocities):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
119 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity,
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
120 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
121 return predictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
122
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
123 class EvasiveActionPredictionParameters(PredictionParameters):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
124 def __init__(self, maxSpeed, nPredictedTrajectories, minAcceleration, maxAcceleration, maxSteering, useFeatures = False):
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
125 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
126 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
127 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
128 name = 'evasive action'
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
129 PredictionParameters.__init__(self, name, maxSpeed)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
130 self.nPredictedTrajectories = nPredictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
131 self.minAcceleration = minAcceleration
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
132 self.maxAcceleration = maxAcceleration
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
133 self.maxSteering = maxSteering
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
134 self.useFeatures = useFeatures
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
135 self.accelerationDistribution = lambda: random.triangular(self.minAcceleration,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
136 self.maxAcceleration, 0.)
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
137 self.steeringDistribution = lambda: random.triangular(-self.maxSteering,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
138 self.maxSteering, 0.)
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
139
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
140 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
141 return PredictionParameters.__str__(self)+' {0} {1} {2} {3}'.format(self.nPredictedTrajectories, self.minAcceleration, self.maxAcceleration, self.maxSteering)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
142
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
143 def generatePredictedTrajectories(self, obj, instant):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
144 predictedTrajectories = []
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
145 if self.useFeatures:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
146 features = [f for f in obj.features if f.existsAtInstant(instant)]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
147 positions = [f.getPositionAtInstant(instant) for f in features]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
148 velocities = [f.getVelocityAtInstant(instant) for f in features]
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
149 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
150 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
151 velocities = [obj.getVelocityAtInstant(instant)]
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
152 for i in xrange(self.nPredictedTrajectories):
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
153 for initialPosition,initialVelocity in zip(positions, velocities):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
154 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
155 initialVelocity,
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
156 moving.NormAngle(self.accelerationDistribution(),
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
157 self.steeringDistribution()),
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
158 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
159 return predictedTrajectories
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
160
259
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
161 class SafetyPoint(moving.Point):
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
162 '''Can represent a collision point or crossing zone
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
163 with respective safety indicator, TTC or pPET'''
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
164 def __init__(self, p, probability = 1., indicator = -1):
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
165 self.x = p.x
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
166 self.y = p.y
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
167 self.probability = probability
259
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
168 self.indicator = indicator
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
169
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
170 def __str__(self):
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
171 return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator)
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
172
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
173 @staticmethod
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
174 def save(out, points, predictionInstant, objNum1, objNum2):
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
175 for p in points:
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
176 out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p))
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
177
259
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
178 def computeExpectedIndicator(points):
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
179 from numpy import sum
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
180 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
181
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
182 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
183 '''Computes the first instant at which two predicted trajectories are within some distance threshold'''
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
184 t = 1
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
185 p1 = predictedTrajectory1.predictPosition(t)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
186 p2 = predictedTrajectory2.predictPosition(t)
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
187 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
188 p1 = predictedTrajectory1.predictPosition(t)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
189 p2 = predictedTrajectory2.predictPosition(t)
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
190 t += 1
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
191 return t, p1, p2
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
192
295
ba29bd82bd04 added option to disable computation of crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 291
diff changeset
193 def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
194 '''returns the lists of collision points and crossing zones
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
195
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
196 Check: Predicting all the points together, as if they represent the whole vehicle'''
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
197 predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant)
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
198 predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant)
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
199
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
200 collisionPoints = []
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
201 crossingZones = []
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
202 for et1 in predictedTrajectories1:
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
203 for et2 in predictedTrajectories2:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
204 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
205
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
206 if t <= timeHorizon:
290
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 289
diff changeset
207 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
295
ba29bd82bd04 added option to disable computation of crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 291
diff changeset
208 elif computeCZ: # check if there is a crossing zone
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
209 # TODO? zone should be around the points at which the traj are the closest
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
210 # look for CZ at different times, otherwise it would be a collision
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
211 # an approximation would be to look for close points at different times, ie the complementary of collision points
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
212 cz = None
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
213 t1 = 0
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
214 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
215 t2 = 0
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
216 while not cz and t2 < timeHorizon:
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
217 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
218 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
219 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
220 if cz:
259
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
221 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
222 t2 += 1
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
223 t1 += 1
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
224
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
225 if debug:
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
226 from matplotlib.pyplot import figure, axis, title
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
227 figure()
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
228 for et in predictedTrajectories1:
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
229 et.predictPosition(timeHorizon)
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
230 et.draw('rx')
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
231
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
232 for et in predictedTrajectories2:
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
233 et.predictPosition(timeHorizon)
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
234 et.draw('bx')
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
235 obj1.draw('r')
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
236 obj2.draw('b')
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
237 title('instant {0}'.format(i))
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
238 axis('equal')
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
239
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
240 return collisionPoints, crossingZones
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
241
296
586ead03fc00 added option to disable computation of crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 295
diff changeset
242 def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
243 '''Computes all crossing and collision points at each common instant for two road users. '''
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
244 collisionPoints={}
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
245 crossingZones={}
264
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
246 if timeInterval:
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
247 commonTimeInterval = timeInterval
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
248 else:
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
249 commonTimeInterval = obj1.commonTimeInterval(obj2)
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
250 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
296
586ead03fc00 added option to disable computation of crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 295
diff changeset
251 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
252
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
253 return collisionPoints, crossingZones
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
254
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
255 def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
256 '''Computes only collision probabilities
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
257 Returns for each instant the collision probability and number of samples drawn'''
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
258 collisionProbabilities = {}
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
259 if timeInterval:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
260 commonTimeInterval = timeInterval
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
261 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
262 commonTimeInterval = obj1.commonTimeInterval(obj2)
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
263 for i in list(commonTimeInterval)[:-1]:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
264 nCollisions = 0
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
265 print(obj1.num, obj2.num, i)
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
266 predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, i)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
267 predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, i)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
268 for et1 in predictedTrajectories1:
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
269 for et2 in predictedTrajectories2:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
270 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
271 if t <= timeHorizon:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
272 nCollisions += 1
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
273 # take into account probabilities ??
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
274 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2))
289
e56c34c1ebac refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 271
diff changeset
275 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples]
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
276
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
277 if debug:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
278 from matplotlib.pyplot import figure, axis, title
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
279 figure()
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
280 for et in predictedTrajectories1:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
281 et.predictPosition(timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
282 et.draw('rx')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
283
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
284 for et in predictedTrajectories2:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
285 et.predictPosition(timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
286 et.draw('bx')
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
287 obj1.draw('r')
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
288 obj2.draw('b')
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
289 title('instant {0}'.format(i))
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
290 axis('equal')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
291
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
292 return collisionProbabilities
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
293
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
294
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
295 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
296 import doctest
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
297 import unittest
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
298 suite = doctest.DocFileSuite('tests/prediction.txt')
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
299 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
300 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
301 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
302 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
303