Mercurial > hg > nsaunier > traffic-intelligence

annotate scripts/safety-analysis.py @ 383:0ce2210790b1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
fixed stupid naming bug
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 22 Jul 2013 11:46:35 -0400
parents 619ae9a9a788
children 91679eb2ff2c
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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