changeset 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 9c1818a71c9c
files python/events.py scripts/safety-analysis.py
diffstat 2 files changed, 66 insertions(+), 9 deletions(-) [+]
line wrap: on
line diff
--- a/python/events.py	Sun Jun 16 23:44:36 2013 -0400
+++ b/python/events.py	Mon Jun 17 16:26:11 2013 -0400
@@ -87,12 +87,27 @@
                 minDistance[instant] = distance.min()
             self.addIndicator(indicators.SeverityIndicator('Minimum Distance', minDistance))
 
-    def computeCollisionPoints(self, predictionParameters, collisionDistanceThreshold, timeHorizon):
-        if self.roadUser1.features and self.roadUser2.features:
-            collisionPoints = prediction.computeCollisions(self.roadUser1, self.roadUser2, predictionParameters, collisionDistanceThreshold, timeHorizon)
-            self.addIndicator(indicators.SeverityIndicator('Collision Points', collisionPoints))	
+    def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
+        '''Computes all crossing and collision points at each common instant for two road users. '''
+        self.collisionPoints={}
+        TTCs = {}
+        if computeCZ:
+            self.crossingZones={}
+
+        if timeInterval:
+            commonTimeInterval = timeInterval
         else:
-            print('Features not associated with objects')
+            commonTimeInterval = self.timeInterval
+        for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
+            self.collisionPoints[i], self.crossingZones[i] = prediction.computeCrossingsCollisionsAtInstant(i, self.roadUser1, self.roadUser2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
+            TTCs[i] = prediction.computeExpectedIndicator(self.collisionPoints[i])
+        self.addIndicator(indicators.SeverityIndicator('TTC', TTCs))
+        
+        if computeCZ:
+            pPETs = {}
+            for i in list(commonTimeInterval)[:-1]:
+                pPETs[i] = prediction.computeExpectedIndicator(self.crossingZones[i])
+                self.addIndicator(indicators.SeverityIndicator('pPET', pPETs))
 
     def addVideoFilename(self,videoFilename):
         self.videoFilename= videoFilename	
--- a/scripts/safety-analysis.py	Sun Jun 16 23:44:36 2013 -0400
+++ b/scripts/safety-analysis.py	Mon Jun 17 16:26:11 2013 -0400
@@ -1,5 +1,7 @@
 #! /usr/bin/env python
 
+import utils, storage, prediction, events
+
 import sys, argparse
 
 import matplotlib.pyplot as plt
@@ -8,17 +10,57 @@
 from ConfigParser import ConfigParser
 
 parser = argparse.ArgumentParser(description='The program processes indicators for all pairs of road users in the scene')
+parser.add_argument('configFilename', help = 'name of the configuration file')
+# parser.add_argument('-c', help = 'name of the configuration file') # 
 args = parser.parse_args()
 print(args)
 # TODO work on the way to indicate an interaction definition
 
-if False:#len(args)>0: # consider there is a configuration file
-    params = utils.TrackingParameters()
-    params.loadConfigFile(args[0])
+# if False: # test if there is a configuration file?
+params = utils.TrackingParameters()
+params.loadConfigFile(args.configFilename)
+
+# configuration parameters # TODO from command line
+frameRate = 15  # frame per second
+maxSpeed = 90/3.6/frameRate # speed limit 50 km/h for urban envt, 90km/hr = 25 m/sec for highways
+timeHorizon= frameRate*5 # prediction time Horizon = 1.5 s (reaction time) (5 second)
+collisionDistanceThreshold= 1.8 # m
+computeCZ = True
+
+# parameters for prediction methods
+constantVelocityPredictionParameters = prediction.ConstantPredictionParameters(maxSpeed)
+
+normalAdaptationPredictionParameters = prediction.NormalAdaptationPredictionParameters(maxSpeed, 100, 2./frameRate**2, # m/s2
+                                                                                       0.2/frameRate) # rad/s
+
+featurePredictionParameters = prediction.PointSetPredictionParameters(maxSpeed)
+
+evasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(maxSpeed, 100, -9.1/frameRate**2, # m/s2
+                                                                                 4.3/frameRate**2, # m/s2
+                                                                                 0.5/frameRate, # rad/s
+                                                                                 False)
+
+featureEvasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(maxSpeed, 10, -9.1/frameRate**2, # m/s2
+                                                                                        4.3/frameRate**2, # m/s2
+                                                                                        0.5/frameRate, # rad/s
+                                                                                        True)
 
 
 
+objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object')
+# features = storage.loadTrajectoriesFromSqlite('amherst-10.sqlite','feature') # needed if normal adaptation
+
+interactions = events.createInteractions(objects)
+for inter in interactions[:2]:
+    inter.computeCrossingsCollisions(constantVelocityPredictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ)
+
+plt.figure()
+plt.axis('equal')
+for inter in interactions[:2]:
+    for collisionPoints in inter.collisionPoints.values():
+        for cp in collisionPoints:
+            plot([cp.x], [cp.y], 'x')
 
 # for the demo, output automatically a map
-
+# possibility to process longitudinal coords only