Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1085:7853106677b7

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 25 Sep 2018 17:08:37 -0400
parents 346b41cbc81a
children 8734742c08c0
comparison
equal deleted inserted replaced
1084:1a7e0b2c858b 1085:7853106677b7
1059 return lcss.compute(t1, t2) 1059 return lcss.compute(t1, t2)
1060 1060
1061 class CurvilinearTrajectory(Trajectory): 1061 class CurvilinearTrajectory(Trajectory):
1062 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements 1062 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements
1063 longitudinal coordinate is stored as first coordinate (exterior name S) 1063 longitudinal coordinate is stored as first coordinate (exterior name S)
1064 lateral coordiante is stored as second coordinate''' 1064 lateral coordinate is stored as second coordinate
1065 the third "lane" coordinate is for an alignment id,
1066 whether explicit for a list/dict of alignments,
1067 or implicit for a road with lane numbers'''
1065 1068
1066 def __init__(self, S = None, Y = None, lanes = None): 1069 def __init__(self, S = None, Y = None, lanes = None):
1067 if S is None or Y is None or len(S) != len(Y): 1070 if S is None or Y is None or len(S) != len(Y):
1068 self.positions = [[],[]] 1071 self.positions = [[],[]]
1069 if S is not None and Y is not None and len(S) != len(Y): 1072 if S is not None and Y is not None and len(S) != len(Y):
1070 print("S and Y coordinates of different lengths\nInitializing to empty lists") 1073 print("S and Y coordinates of different lengths\nInitializing to empty lists")
1071 else: 1074 else:
1072 self.positions = [S,Y] 1075 self.positions = [S,Y]
1073 if lanes is None or len(lanes) != self.length(): 1076 if lanes is None or len(lanes) != self.length():
1074 self.lanes = [] 1077 self.lanes = [None]*int(self.length())
1075 else: 1078 else:
1076 self.lanes = lanes 1079 self.lanes = lanes
1077 1080
1081 @staticmethod
1082 def generate(s, v, nPoints, lane, y = 0):
1083 '''s is initial position, v is velocity
1084 0 in lateral coordinate by default
1085 TODO 2D velocity for lane change?'''
1086 S = [s]
1087 for i in range(nPoints-1):
1088 S.append(S[-1]+v)
1089 Y = [y]*nPoints
1090 lanes = [lane]*nPoints
1091 return CurvilinearTrajectory(S, Y, lanes)
1092
1078 def __getitem__(self,i): 1093 def __getitem__(self,i):
1079 if isinstance(i, int): 1094 if isinstance(i, int):
1080 return [self.positions[0][i], self.positions[1][i], self.lanes[i]] 1095 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
1081 else: 1096 else:
1082 raise TypeError("Invalid argument type.") 1097 raise TypeError("Invalid argument type.")
1086 return self.getXCoordinates() 1101 return self.getXCoordinates()
1087 1102
1088 def getLanes(self): 1103 def getLanes(self):
1089 return self.lanes 1104 return self.lanes
1090 1105
1091 def addPositionSYL(self, s, y, lane): 1106 def addPositionSYL(self, s, y, lane = None):
1092 self.addPositionXY(s,y) 1107 self.addPositionXY(s,y)
1093 self.lanes.append(lane) 1108 self.lanes.append(lane)
1094 1109
1095 def addPosition(self, p): 1110 def addPosition(self, p):
1096 'Adds position in the point format for curvilinear of list with 3 values' 1111 'Adds position in the point format for curvilinear of list with 3 values'
1104 def differentiate(self, doubleLastPosition = False): 1119 def differentiate(self, doubleLastPosition = False):
1105 diff = CurvilinearTrajectory() 1120 diff = CurvilinearTrajectory()
1106 p1 = self[0] 1121 p1 = self[0]
1107 for i in range(1, self.length()): 1122 for i in range(1, self.length()):
1108 p2 = self[i] 1123 p2 = self[i]
1109 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2]) 1124 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1])
1110 p1=p2 1125 p1=p2
1111 if doubleLastPosition and self.length() > 1: 1126 if doubleLastPosition and self.length() > 1:
1112 diff.addPosition(diff[-1]) 1127 diff.addPosition(diff[-1])
1113 return diff 1128 return diff
1114 1129