Mercurial Hosting > traffic-intelligence
annotate scripts/safety-analysis.py @ 338:f3aceea2afbb
first safety analysis script
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 17 Jun 2013 16:26:11 -0400 |
parents | dc2e68e936c7 |
children | 74e437ab5f11 |
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 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
30 # parameters for prediction methods |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
31 constantVelocityPredictionParameters = prediction.ConstantPredictionParameters(maxSpeed) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
32 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
33 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
|
34 0.2/frameRate) # rad/s |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
35 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
36 featurePredictionParameters = prediction.PointSetPredictionParameters(maxSpeed) |
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 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
|
39 4.3/frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
40 0.5/frameRate, # rad/s |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
41 False) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
42 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
43 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
|
44 4.3/frameRate**2, # m/s2 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
45 0.5/frameRate, # rad/s |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
46 True) |
333
c9201f6b143a
moved the config parser to utils
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
328
diff
changeset
|
47 |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
48 |
335
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
334
diff
changeset
|
49 |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
50 objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object') |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
51 # 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
|
52 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
53 interactions = events.createInteractions(objects) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
54 for inter in interactions[:2]: |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
55 inter.computeCrossingsCollisions(constantVelocityPredictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ) |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
56 |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
57 plt.figure() |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
58 plt.axis('equal') |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
59 for inter in interactions[:2]: |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
60 for collisionPoints in inter.collisionPoints.values(): |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
61 for cp in collisionPoints: |
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
62 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
|
63 |
3950bfe22768
added functions to export trajectories to csv
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
334
diff
changeset
|
64 # for the demo, output automatically a map |
338
f3aceea2afbb
first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
337
diff
changeset
|
65 # possibility to process longitudinal coords only |
328
5e43b7389c25
script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff
changeset
|
66 |