Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/prediction.py @ 1215:1b472cddf9b1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
updated reading by interpolating missing frames
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 05 May 2023 22:52:25 -0400
parents 01c24c1cdb70
children ca70a79688ae
comparison
equal deleted inserted replaced
1214:01c24c1cdb70 1215:1b472cddf9b1
556 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) 556 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2)
557 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) 557 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
558 558
559 if collisionTimeInterval.empty(): 559 if collisionTimeInterval.empty():
560 if computeCZ: 560 if computeCZ:
561 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] 561 crossingZones.append(SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2)))
562 else: 562 else:
563 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] 563 collisionPoints.append(SafetyPoint(intersection, 1., collisionTimeInterval.center()))
564 564
565 if debug and intersection is not None: 565 if debug and intersection is not None:
566 from matplotlib.pyplot import plot, figure, axis, title 566 from matplotlib.pyplot import plot, figure, axis, title
567 figure() 567 figure()
568 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') 568 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')
584 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None, useCurvilinear) 584 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None, useCurvilinear)
585 585
586 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): 586 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
587 'TODO compute pPET' 587 'TODO compute pPET'
588 collisionPoints = [] 588 collisionPoints = []
589 crossingZones = [] 589 if computeCZ:
590 crossingZones = []
591 else:
592 crossingZones = None
590 593
591 if self.useCurvilinear: 594 if self.useCurvilinear:
592 pass # Lionel 595 pass # Lionel
593 else: 596 else:
594 p1 = obj1.getPositionAtInstant(currentInstant) 597 p1 = obj1.getPositionAtInstant(currentInstant)
598 #intersection = moving.intersection(p1, p1+v1, p2, p2+v2) 601 #intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
599 602
600 if not moving.Point.parallel(v1, v2): 603 if not moving.Point.parallel(v1, v2):
601 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) 604 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
602 if ttc is not None: 605 if ttc is not None:
603 collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)] 606 collisionPoints.append(SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc))
604 else: 607 else:
605 pass # compute pPET 608 pass # compute pPET
606 return collisionPoints, crossingZones 609 return collisionPoints, crossingZones
607 610
608 class CVExactPolyPredictionParameters(PredictionParameters): 611 class CVExactPolyPredictionParameters(PredictionParameters):
614 PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None) 617 PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None)
615 618
616 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): 619 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
617 'TODO compute pPET' 620 'TODO compute pPET'
618 collisionPoints = [] 621 collisionPoints = []
619 crossingZones = [] 622 if computeCZ:
623 crossingZones = []
624 else:
625 crossingZones = None
620 626
621 if self.useCurvilinear: 627 if self.useCurvilinear:
622 pass # Lionel 628 pass # Lionel
623 else: 629 else:
624 poly1 = obj1.getBoundingPolygon(currentInstant) 630 poly1 = obj1.getBoundingPolygon(currentInstant)
627 v2 = obj2.getVelocityAtInstant(currentInstant) 633 v2 = obj2.getVelocityAtInstant(currentInstant)
628 634
629 if not moving.Point.parallel(v1, v2): 635 if not moving.Point.parallel(v1, v2):
630 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2) 636 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2)
631 if ttc is not None: 637 if ttc is not None:
632 collisionPoints = [SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)] 638 collisionPoints.append(SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc))
633 else: 639 else:
634 pass # compute pPET 640 pass # compute pPET
635 return collisionPoints, crossingZones 641 return collisionPoints, crossingZones
636 642
637 643