forked from lawbreaker2022/LawBreaker-SourceCode
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmap.py
153 lines (123 loc) · 5.56 KB
/
map.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
import copy
import json
import warnings
import numpy as np
from shapely.geometry import Point, LineString
directory = 'map/'
class get_map_info:
def __init__(self, map_name):
self.file = directory + map_name + ".json"
self.lane_config = dict()
self.lane_waypoints = dict()
self.crosswalk_config = dict()
self.lane_predecessor = dict()
self.lane_successor = dict()
with open(self.file) as f:
map_config = json.load(f)
lane = map_config['laneList']
crosswalk = map_config['crosswalkList']
for i in range(len(lane)):
lane_id = lane[i]['id']['id']
lane_length = lane[i]['length']
self.lane_config[lane_id] = lane_length
self.lane_waypoints[lane_id] = []
for _i in range(len(lane[i]['centralCurve']['segmentList'])):
lane_segment_point = lane[i]['centralCurve']['segmentList'][_i]['lineSegment']['pointList']
for k in range(len(lane_segment_point)):
_wp_k = lane_segment_point[k]
self.lane_waypoints[lane_id].append(np.array([_wp_k['x'], _wp_k['y']]))
predecessor = lane[i]['predecessorIdList']
self.lane_predecessor[lane_id] = []
for j in range(len(predecessor)):
self.lane_predecessor[lane_id].append(predecessor[j]['id'])
successor = lane[i]['successorIdList']
self.lane_successor[lane_id] = []
for k in range(len(successor)):
self.lane_successor[lane_id].append(successor[k]['id'])
for j in range(len(crosswalk)):
crosswalk_polygon = crosswalk[j]['polygon']['pointList']
if len(crosswalk_polygon) != 4:
print('Needs four points to describe a crosswalk!')
exit()
crosswalk_points = [(crosswalk_polygon[0]['x'], crosswalk_polygon[0]['y']),
(crosswalk_polygon[1]['x'], crosswalk_polygon[1]['y']),
(crosswalk_polygon[2]['x'], crosswalk_polygon[2]['y']),
(crosswalk_polygon[3]['x'], crosswalk_polygon[3]['y'])
]
self.crosswalk_config['crosswalk'+str(j+1)] = crosswalk_points
def get_lane_config(self):
return self.lane_config
def get_successor_lanes(self, lane_name):
return self.lane_successor[lane_name]
def get_predecessor_lanes(self, lane_name):
return self.lane_predecessor[lane_name]
def get_crosswalk_config(self):
return self.crosswalk_config
def get_position(self, lane_position):
## lane_position = [lane_id, offset]
lane_id = lane_position[0]
offset = lane_position[1]
waypoint = self.lane_waypoints[lane_id]
_distance = 0
for i in range(len(waypoint)-1):
wp1 = waypoint[i]
wp2 = waypoint[i+1]
_dis_wp1_2 = np.linalg.norm(wp1 - wp2)
if _distance + _dis_wp1_2 > offset:
current_dis = offset - _distance
k = current_dis / _dis_wp1_2
x = wp1[0] + (wp2[0] - wp1[0])*k
y = wp1[1] + (wp2[1] - wp1[1])*k
return (x, y, 0)
_distance += _dis_wp1_2
if i == len(waypoint)-2:
warnings.warn("The predefined position is out of the given lane, set to the end of the lane.")
return (wp2[0], wp2[1], 0)
def position2lane(self, position):
_dis = float('inf')
_point = Point(position)
for item in self.lane_waypoints.keys():
_line = LineString(self.lane_waypoints[item])
_current_dis = _point.distance(_line)
if _current_dis < _dis:
_dis = _current_dis
_lane_name = item
return _lane_name
def get_global_position(self, position, local_position):
'''
Args:
position: (x, y)
local_position: position in the vehicle frame
Returns:
'''
# _dis = float('inf')
_point = Point(position)
# for item in self.lane_waypoints.keys():
# _line = LineString(self.lane_waypoints[item])
# _current_dis = _point.distance(_line)
# if _current_dis < _dis:
# _dis = _current_dis
# _lane_name = item
_lane_name = self.position2lane(position)
_lane_waypoint = self.lane_waypoints[_lane_name]
_in_lane_dis = float('inf')
_waypoint_index = 0
for i in range(len(_lane_waypoint)-1):
_segment_line = LineString([_lane_waypoint[i], _lane_waypoint[i+1]])
_current_dis = _point.distance(_segment_line)
if _current_dis < _in_lane_dis:
_in_lane_dis = _current_dis
_waypoint_index = i
direction = _lane_waypoint[_waypoint_index+1] - _lane_waypoint[_waypoint_index]
cos_theta = direction[0] / np.linalg.norm(direction)
sin_theta = direction[1] / np.linalg.norm(direction)
x1 = local_position[0] * cos_theta - local_position[1] * sin_theta
y1 = local_position[0] * sin_theta + local_position[1] * cos_theta
return (x1 + position[0], y1 + position[1])
if __name__ == "__main__":
map = "san_francisco"
map_info = get_map_info(map)
map_info.get_lane_config()
lane_point = ["lane_231", 100]
p = map_info.get_position(lane_point)
print(p)