Mercurial > hg > nsaunier > traffic-intelligence

annotate scripts/safety-analysis.py @ 343:74e437ab5f11

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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
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
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