Mercurial Hosting > traffic-intelligence
annotate scripts/safety-analysis.py @ 375:2ea8584aa80a
making indicator LCSS work
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 17 Jul 2013 01:29:25 -0400 |
parents | 619ae9a9a788 |
children | 91679eb2ff2c |
rev | line source |
---|---|
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
1 #! /usr/bin/env python |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
2 |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
3 import utils, storage, prediction, events, moving |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
4 |
337
dc2e68e936c7
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
335
diff
changeset
|
5 import sys, argparse |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
6 |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
7 import matplotlib.pyplot as plt |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
8 import numpy as np |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
9 |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
10 # todo: very slow if too many predicted trajectories |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
11 # add computation of probality of unsucessful evasive action |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
12 |
337
dc2e68e936c7
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
335
diff
changeset
|
13 parser = argparse.ArgumentParser(description='The program processes indicators for all pairs of road users in the scene') |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
14 parser.add_argument('--cfg', dest = 'configFilename', help = 'name of the configuration file') |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
15 parser.add_argument('--prediction-method', dest = 'predictionMethod', help = 'prediction method (constant velocity (vector computation), constant velocity, normal adaptation, point set prediction)', choices = ['cvd', 'cv', 'na', 'ps']) |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
16 parser.add_argument('--display-cp', dest = 'displayCollisionPoints', help = 'display collision points') |
337
dc2e68e936c7
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
335
diff
changeset
|
17 args = parser.parse_args() |
348
c64a4f889b97
added safety analysis options to feature tracking (with default values for backward compatibility)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
344
diff
changeset
|
18 |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
19 params = utils.TrackingParameters() |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
20 params.loadConfigFile(args.configFilename) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
21 |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
22 # parameters for prediction methods |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
23 if args.predictionMethod: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
24 predictionMethod = args.predictionMethod |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
25 else: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
26 predictionMethod = params.predictionMethod |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
27 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
28 if predictionMethod == 'cvd': |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
29 predictionParameters = prediction.CVDirectPredictionParameters(params.maxPredictedSpeed) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
30 elif predictionMethod == 'cv': |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
31 predictionParameters = prediction.ConstantPredictionParameters(params.maxPredictedSpeed) |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
32 elif predictionMethod == 'na': |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
33 predictionParameters = prediction.NormalAdaptationPredictionParameters(params.maxPredictedSpeed, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
34 params.nPredictedTrajectories, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
35 params.maxAcceleration, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
36 params.maxSteering, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
37 params.useFeaturesForPrediction) |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
38 elif predictionMethod == 'ps': |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
39 predictionParameters = prediction.PointSetPredictionParameters(params.nPredictedTrajectories, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
40 params.maxPredictedSpeed) |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
41 else: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
42 print('Prediction method {} is not valid. See help.'.format(predictionMethod)) |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
43 sys.exit() |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
44 |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
45 evasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(params.maxPredictedSpeed, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
46 params.nPredictedTrajectories, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
47 params.minAcceleration, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
48 params.maxAcceleration, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
348
diff
changeset
|
49 params.maxSteering, |
351
891858351bcb
added new parameter (use features) for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
50 params.useFeaturesForPrediction) |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
51 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
52 objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object') |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
53 if params.useFeaturesForPrediction: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
54 features = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'feature') # needed if normal adaptation |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
55 for obj in objects: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
56 obj.setFeatures(features) |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
57 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
58 interactions = events.createInteractions(objects) |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
59 for inter in interactions: |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
60 inter.computeIndicators() |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
61 inter.computeCrossingsCollisions(predictionParameters, params.collisionDistance, params.predictionTimeHorizon, params.crossingZones) |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
62 |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
63 storage.saveIndicators(params.databaseFilename, interactions) |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
64 |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
65 if args.displayCollisionPoints: |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
66 plt.figure() |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
67 allCollisionPoints = [] |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
68 for inter in interactions: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
351
diff
changeset
|
69 for collisionPoints in inter.collisionPoints.values(): |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
70 allCollisionPoints += collisionPoints |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
71 moving.Point.plotAll(allCollisionPoints) |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
352
diff
changeset
|
72 plt.axis('equal') |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
73 |