Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1085:7853106677b7
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 |