Mercurial Hosting > traffic-intelligence
annotate scripts/safety-analysis.py @ 343:74e437ab5f11
first version of indicator loading code
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 21 Jun 2013 15:28:59 -0400 |
parents | f3aceea2afbb |
children | 14a2405f54f8 |
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 |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
3 import utils, storage, prediction, events |
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 |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
10 from ConfigParser import ConfigParser |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
11 |
337
dc2e68e936c7
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
335
diff
changeset
|
12 parser = argparse.ArgumentParser(description='The program processes indicators for all pairs of road users in the scene') |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
13 parser.add_argument('configFilename', help = 'name of the configuration file') |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
14 # parser.add_argument('-c', help = 'name of the configuration file') # |
337
dc2e68e936c7
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
335
diff
changeset
|
15 args = parser.parse_args() |
dc2e68e936c7
work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
335
diff
changeset
|
16 print(args) |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
17 # TODO work on the way to indicate an interaction definition |
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
18 |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
19 # if False: # test if there is a configuration file? |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
20 params = utils.TrackingParameters() |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
21 params.loadConfigFile(args.configFilename) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
22 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
23 # configuration parameters # TODO from command line |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
24 frameRate = 15 # frame per second |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
25 maxSpeed = 90/3.6/frameRate # speed limit 50 km/h for urban envt, 90km/hr = 25 m/sec for highways |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
26 timeHorizon= frameRate*5 # prediction time Horizon = 1.5 s (reaction time) (5 second) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
27 collisionDistanceThreshold= 1.8 # m |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
28 computeCZ = True |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
29 |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
30 display = False |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
31 |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
32 # parameters for prediction methods |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
33 constantVelocityPredictionParameters = prediction.ConstantPredictionParameters(maxSpeed) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
34 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
35 normalAdaptationPredictionParameters = prediction.NormalAdaptationPredictionParameters(maxSpeed, 100, 2./frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
36 0.2/frameRate) # rad/s |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
37 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
38 featurePredictionParameters = prediction.PointSetPredictionParameters(maxSpeed) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
39 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
40 evasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(maxSpeed, 100, -9.1/frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
41 4.3/frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
42 0.5/frameRate, # rad/s |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
43 False) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
44 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
45 featureEvasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(maxSpeed, 10, -9.1/frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
46 4.3/frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
47 0.5/frameRate, # rad/s |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
48 True) |
333
c9201f6b143a
moved the config parser to utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
328
diff
changeset
|
49 |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
50 |
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
334
diff
changeset
|
51 |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
52 objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object') |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
53 # features = storage.loadTrajectoriesFromSqlite('amherst-10.sqlite','feature') # needed if normal adaptation |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
54 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
55 interactions = events.createInteractions(objects) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
56 for inter in interactions[:2]: |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
57 inter.computeIndicators() |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
58 # inter.computeCrossingsCollisions(constantVelocityPredictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ) |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
59 |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
60 storage.saveIndicators(params.databaseFilename, interactions) |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
61 |
343
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
62 if display: |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
63 plt.figure() |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
64 plt.axis('equal') |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
65 for inter in interactions[:2]: |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
66 for collisionPoints in inter.collisionPoints.values(): |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
67 for cp in collisionPoints: |
74e437ab5f11
first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
338
diff
changeset
|
68 plot([cp.x], [cp.y], 'x') |
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
334
diff
changeset
|
69 |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
334
diff
changeset
|
70 # for the demo, output automatically a map |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
71 # possibility to process longitudinal coords only |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
72 |