Mercurial Hosting > traffic-intelligence
annotate python/prediction.py @ 398:3399bd48cb40
Ajout d'une méthode pour obtenir le nombre de FPS
Méthode de capture des trames vidéos plus résistante aux erreur
Utilisation d'un dictionnaire pour les fichier de configuration afin de garder le nom des sections
author | Jean-Philippe Jodoin <jpjodoin@gmail.com> |
---|---|
date | Mon, 29 Jul 2013 13:46:07 -0400 |
parents | 91679eb2ff2c |
children | 55b424d98b68 |
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 |
336
124f85c6cfae
modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
300
diff
changeset
|
40 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1., maxSpeed = None): |
250
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 ''' |
336
124f85c6cfae
modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
300
diff
changeset
|
52 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): |
256
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 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
66 class SafetyPoint(moving.Point): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
67 '''Can represent a collision point or crossing zone |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
68 with respective safety indicator, TTC or pPET''' |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
69 def __init__(self, p, probability = 1., indicator = -1): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
70 self.x = p.x |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
71 self.y = p.y |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
72 self.probability = probability |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
73 self.indicator = indicator |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
74 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
75 def __str__(self): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
76 return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
77 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
78 @staticmethod |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
79 def save(out, points, predictionInstant, objNum1, objNum2): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
80 for p in points: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
81 out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p)) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
82 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
83 @staticmethod |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
84 def computeExpectedIndicator(points): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
85 from numpy import sum |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
86 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
87 |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
88 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
89 '''Computes the first instant at which two predicted trajectories are within some distance threshold''' |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
90 t = 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
91 p1 = predictedTrajectory1.predictPosition(t) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
92 p2 = predictedTrajectory2.predictPosition(t) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
93 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
94 p1 = predictedTrajectory1.predictPosition(t) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
95 p2 = predictedTrajectory2.predictPosition(t) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
96 t += 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
97 return t, p1, p2 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
98 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
99 class PredictionParameters: |
266
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
100 def __init__(self, name, maxSpeed): |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
101 self.name = name |
266
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
102 self.maxSpeed = maxSpeed |
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
103 |
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
104 def __str__(self): |
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
105 return '{0} {1}'.format(self.name, self.maxSpeed) |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
106 |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
107 def generatePredictedTrajectories(self, obj, instant): |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
108 return [] |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
109 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
110 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
111 '''returns the lists of collision points and crossing zones''' |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
112 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
113 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
114 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
115 collisionPoints = [] |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
116 crossingZones = [] |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
117 for et1 in predictedTrajectories1: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
118 for et2 in predictedTrajectories2: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
119 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
120 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
121 if t <= timeHorizon: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
122 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
123 elif computeCZ: # check if there is a crossing zone |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
124 # TODO? zone should be around the points at which the traj are the closest |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
125 # look for CZ at different times, otherwise it would be a collision |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
126 # an approximation would be to look for close points at different times, ie the complementary of collision points |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
127 cz = None |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
128 t1 = 0 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
129 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
130 t2 = 0 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
131 while not cz and t2 < timeHorizon: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
132 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
133 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
134 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
135 if cz: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
136 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
137 t2 += 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
138 t1 += 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
139 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
140 if debug: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
141 from matplotlib.pyplot import figure, axis, title |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
142 figure() |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
143 for et in predictedTrajectories1: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
144 et.predictPosition(timeHorizon) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
145 et.draw('rx') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
146 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
147 for et in predictedTrajectories2: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
148 et.predictPosition(timeHorizon) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
149 et.draw('bx') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
150 obj1.draw('r') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
151 obj2.draw('b') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
152 title('instant {0}'.format(currentInstant)) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
153 axis('equal') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
154 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
155 return collisionPoints, crossingZones |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
156 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
157 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None): |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
158 '''Computes all crossing and collision points at each common instant for two road users. ''' |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
159 collisionPoints={} |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
160 crossingZones={} |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
161 if timeInterval: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
162 commonTimeInterval = timeInterval |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
163 else: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
164 commonTimeInterval = obj1.commonTimeInterval(obj2) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
165 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
166 collisionPoints[i], crossingZones[i] = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
167 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
168 return collisionPoints, crossingZones |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
169 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
170 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
171 '''Computes only collision probabilities |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
172 Returns for each instant the collision probability and number of samples drawn''' |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
173 collisionProbabilities = {} |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
174 if timeInterval: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
175 commonTimeInterval = timeInterval |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
176 else: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
177 commonTimeInterval = obj1.commonTimeInterval(obj2) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
178 for i in list(commonTimeInterval)[:-1]: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
179 nCollisions = 0 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
180 print(obj1.num, obj2.num, i) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
181 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
182 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
183 for et1 in predictedTrajectories1: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
184 for et2 in predictedTrajectories2: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
185 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
186 if t <= timeHorizon: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
187 nCollisions += 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
188 # take into account probabilities ?? |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
189 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
190 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
191 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
192 if debug: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
193 from matplotlib.pyplot import figure, axis, title |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
194 figure() |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
195 for et in predictedTrajectories1: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
196 et.predictPosition(timeHorizon) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
197 et.draw('rx') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
198 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
199 for et in predictedTrajectories2: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
200 et.predictPosition(timeHorizon) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
201 et.draw('bx') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
202 obj1.draw('r') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
203 obj2.draw('b') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
204 title('instant {0}'.format(i)) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
205 axis('equal') |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
206 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
207 return collisionProbabilities |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
208 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
209 class ConstantPredictionParameters(PredictionParameters): |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
210 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
|
211 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
|
212 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
213 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
|
214 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
|
215 maxSpeed = self.maxSpeed)] |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
216 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
217 class NormalAdaptationPredictionParameters(PredictionParameters): |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
218 def __init__(self, maxSpeed, nPredictedTrajectories, maxAcceleration, maxSteering, useFeatures = False): |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
219 if useFeatures: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
220 name = 'point set normal adaptation' |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
221 else: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
222 name = 'normal adaptation' |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
223 PredictionParameters.__init__(self, name, maxSpeed) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
224 self.nPredictedTrajectories = nPredictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
225 self.maxAcceleration = maxAcceleration |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
226 self.maxSteering = maxSteering |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
227 self.useFeatures = useFeatures |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
228 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
|
229 self.maxAcceleration, 0.) |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
230 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
|
231 self.maxSteering, 0.) |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
232 |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
233 def __str__(self): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
234 return PredictionParameters.__str__(self)+' {0} {1} {2}'.format(self.nPredictedTrajectories, |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
235 self.maxAcceleration, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
236 self.maxSteering) |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
237 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
238 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
|
239 predictedTrajectories = [] |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
240 if self.useFeatures: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
241 features = [f for f in obj.features if f.existsAtInstant(instant)] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
242 positions = [f.getPositionAtInstant(instant) for f in features] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
243 velocities = [f.getVelocityAtInstant(instant) for f in features] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
244 else: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
245 positions = [obj.getPositionAtInstant(instant)] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
246 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
|
247 for i in xrange(self.nPredictedTrajectories): |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
248 for initialPosition,initialVelocity in zip(positions, velocities): |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
249 predictedTrajectories.append(PredictedTrajectoryNormalAdaptation(initialPosition, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
250 initialVelocity, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
251 self.accelerationDistribution, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
252 self.steeringDistribution, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
253 maxSpeed = self.maxSpeed)) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
254 return predictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
255 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
256 class PointSetPredictionParameters(PredictionParameters): |
269
a9988971aac8
removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
257 # todo generate several trajectories with normal adaptatoins from each position (feature) |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
258 def __init__(self, nPredictedTrajectories, maxSpeed): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
259 PredictionParameters.__init__(self, 'point set', maxSpeed) |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
260 self.nPredictedTrajectories = nPredictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
261 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
262 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
|
263 predictedTrajectories = [] |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
264 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
|
265 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
|
266 velocities = [f.getVelocityAtInstant(instant) for f in features] |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
267 for i in xrange(self.nPredictedTrajectories): |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
268 for initialPosition,initialVelocity in zip(positions, velocities): |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
269 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
270 maxSpeed = self.maxSpeed)) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
271 return predictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
272 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
273 class EvasiveActionPredictionParameters(PredictionParameters): |
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
274 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
|
275 if useFeatures: |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
276 name = 'point set evasive action' |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
277 else: |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
278 name = 'evasive action' |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
279 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
|
280 self.nPredictedTrajectories = nPredictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
281 self.minAcceleration = minAcceleration |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
282 self.maxAcceleration = maxAcceleration |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
283 self.maxSteering = maxSteering |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
284 self.useFeatures = useFeatures |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
285 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
|
286 self.maxAcceleration, 0.) |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
287 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
|
288 self.maxSteering, 0.) |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
289 |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
290 def __str__(self): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
291 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
|
292 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
293 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
|
294 predictedTrajectories = [] |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
295 if self.useFeatures: |
267
32e88b513f5c
added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
266
diff
changeset
|
296 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
|
297 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
|
298 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
|
299 else: |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
300 positions = [obj.getPositionAtInstant(instant)] |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
301 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
|
302 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
|
303 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
|
304 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
305 initialVelocity, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
306 moving.NormAngle(self.accelerationDistribution(), |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
307 self.steeringDistribution()), |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
308 maxSpeed = self.maxSpeed)) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
309 return predictedTrajectories |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
310 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
311 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
312 class CVDirectPredictionParameters(PredictionParameters): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
313 '''Prediction parameters of prediction at constant velocity |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
314 using direct computation of the intersecting point''' |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
315 |
387
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
316 def __init__(self): |
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
317 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None) |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
318 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
319 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
320 collisionPoints = [] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
321 crossingZones = [] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
322 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
323 p1 = obj1.getPositionAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
324 p2 = obj2.getPositionAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
325 if (p1-p2).norm2() <= collisionDistanceThreshold: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
326 collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
327 else: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
328 v1 = obj1.getVelocityAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
329 v2 = obj2.getVelocityAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
330 intersection = moving.intersection(p1, p2, v1, v2) |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
331 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
332 if intersection != None: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
333 dp1 = intersection-p1 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
334 dp2 = intersection-p2 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
335 if moving.Point.dot(dp1, v1) > 0 and moving.Point.dot(dp2, v2) > 0: # if the road users are moving towards the intersection |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
336 dist1 = dp1.norm2() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
337 dist2 = dp2.norm2() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
338 s1 = v1.norm2() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
339 s2 = v2.norm2() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
340 halfCollisionDistanceThreshold = collisionDistanceThreshold/2. |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
341 timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
342 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
343 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) |
387
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
344 if computeCZ and collisionTimeInterval.empty(): |
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
345 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] |
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
346 else: |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
347 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
348 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
349 if debug and intersection!= None: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
350 from matplotlib.pyplot import plot, figure, axis, title |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
351 figure() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
352 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
353 plot([p2.x, intersection.x], [p2.y, intersection.y], 'b') |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
354 intersection.draw() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
355 obj1.draw('r') |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
356 obj2.draw('b') |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
357 title('instant {0}'.format(currentInstant)) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
358 axis('equal') |
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
|
359 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
360 return collisionPoints, crossingZones |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
361 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
362 |
269
a9988971aac8
removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
363 |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
364 |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
365 #### |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
366 # Other Methods |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
367 #### |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
368 |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
369 |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
370 |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
371 |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
372 |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
373 if __name__ == "__main__": |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
374 import doctest |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
375 import unittest |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
376 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
|
377 #suite = doctest.DocTestSuite() |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
378 unittest.TextTestRunner().run(suite) |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
379 #doctest.testmod() |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
380 #doctest.testfile("example.txt") |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
381 |