def get_discretized_future_lanes(ego_pos, ego_quat, lane_record, nusc_map, frame='global', ego_speed=2):
lane_discretized_pos_global = np.array(arcline_path_utils.discretize_lane(lane_record, resolution_meters=ego_speed))
#if lane_discretized_pos_global.ndim == 2:
if lane_discretized_pos_global is None or lane_discretized_pos_global.ndim == 0:
lane_discretized_pos_global = np.repeat(np.array(ego_pos)[np.newaxis,:], 10, axis=0)
lane_discretized_pos_global = lane_discretized_pos_global[:,:2]
# else:
# # here !!!!!!!!!
# if frame == 'global':
# lane_discretized_poses = np.repeat()
# elif frame == 'local':
# lane_discretized_poses = lane_discretized_poses_local
lane_discretized_poses_local = convert_global_coords_to_local(lane_discretized_pos_global, np.array(ego_pos), np.array(ego_quat))
# get part of current lane that's in front of the ego
lane_discretized_poses_local_forward_idx = np.argmax(lane_discretized_poses_local[:,1] > 0)
lane_discretized_poses_local = lane_discretized_poses_local[lane_discretized_poses_local_forward_idx:, :]
if frame == 'global':
lane_discretized_poses = convert_local_coords_to_global(lane_discretized_poses_local, ego_pos, ego_quat)
elif frame == 'local':
lane_discretized_poses = lane_discretized_poses_local
return lane_discretized_poses
def populate_lane_dict(
nusc_map: NuScenesMap, root: ET.Element,
data: Dict[str, Iterable[Any]]) -> Dict[str, Iterable[int]]:
"""
Return a map that links a nuscenes 'way' token to all the nodes that are a part of that way.
Also adds all these nodes to the xml map.
"""
lane_dict = defaultdict(list)
# Map x,y value of node to the new(argo) node id in the xml
present_nodes = {} # k:tuple, v: node_id
global_id = 0 # New id for nodes in the xml
# Loop over all lanes in the json to populate the lane to node dictionary
for way in data["lane"] + data["lane_connector"]:
lane_record = nusc_map.get_arcline_path(
way["token"]) # get arcline associated with lane
poses = arcline_path_utils.discretize_lane(
lane_record, LANE_DISCRETIZATION_RESOLUTION_M
) # discretize the lane to given resolution
for pose in poses:
currNode = (pose[0], pose[1])
if currNode not in present_nodes:
node = ET.SubElement(root, "node")
node.set("id", str(global_id))
present_nodes[currNode] = global_id
global_id += 1
node.set("x", str(pose[0]))
node.set("y", str(pose[1]))
lane_dict[way["token"]].append(present_nodes[currNode])
return lane_dict
def get_closest_lane(self, pos, nusc_map):
closest_lane_id = nusc_map.get_closest_lane(pos[0], pos[1], radius=2)
closest_lane_record = nusc_map.get_lane(closest_lane_id)
closest_lane_poses = np.array(
arcline_path_utils.discretize_lane(closest_lane_record,
resolution_meters=1))
closest_lane_data = {
'record': closest_lane_record,
'poses': closest_lane_poses
}
incoming_lane_ids = nusc_map.get_incoming_lane_ids(closest_lane_id)
if incoming_lane_ids is None:
incoming_lane_data = [{
'record': None,
'poses': np.array([pos[:2]])
}]
else:
incoming_lane_data = []
for incoming_lane_id in incoming_lane_ids:
record = nusc_map.get_lane(incoming_lane_id)
poses = np.array(
arcline_path_utils.discretize_lane(record,
resolution_meters=1))
incoming_lane_data.append({'record': record, 'poses': poses})
outgoing_lane_ids = nusc_map.get_outgoing_lane_ids(closest_lane_id)
if outgoing_lane_ids is None:
outgoing_lane_data = [{
'record': None,
'poses': np.array([pos[:2]])
}]
else:
outgoing_lane_data = []
for outgoing_lane_id in outgoing_lane_ids:
record = nusc_map.get_lane(outgoing_lane_id)
poses = np.array(
arcline_path_utils.discretize_lane(record,
resolution_meters=1))
outgoing_lane_data.append({'record': record, 'poses': poses})
return closest_lane_data, incoming_lane_data, outgoing_lane_data
def test_discretize_curved_lane(self):
discrete_path = arcline_path_utils.discretize_lane(self.curved_lane, 5)
answer = np.array([
(421.2419602954602, 1087.9127960414617, 2.739593514975998),
(417.0234337310829, 1089.7051622497897, 2.739830026428688),
(412.80489622772023, 1091.497502717242, 2.739830026428688),
(408.5863587243576, 1093.2898431846943, 2.739830026428688),
(404.3678212209949, 1095.0821836521468, 2.739830026428688),
(400.1492837176322, 1096.874524119599, 2.739830026428688),
(395.93074621426956, 1098.6668645870514, 2.739830026428688),
(391.71428498673856, 1100.4640771829522, 2.7365754617298705),
(391.7142849867393, 1100.464077182952, 2.7365754617298705),
(387.35724292592613, 1101.5723176767192, 3.048461127775915),
(382.87033132963325, 1101.2901176788932, -2.922838513357627),
(378.6864775951582, 1099.6447057425564, -2.610952847311582),
(375.20936805976606, 1096.7948422737907, -2.2990671812655377),
(372.7733659833847, 1093.0160135871613, -2.0002085809158623)
])
np.testing.assert_allclose(answer, discrete_path)
def prepro_closest(self):
print("preprocessing maps...")
info = {}
trees = {}
for map_name, nmap in self.nusc_maps.items():
info[map_name] = {}
for lane in nmap.lane + nmap.lane_connector:
my_lane = nmap.arcline_path_3.get(lane["token"], [])
info[map_name][lane["token"]] = discretize_lane(
my_lane, resolution_meters=0.5)
assert len(info[map_name][lane["token"]]) > 0
data = np.array([
pt for lane in info[map_name] for pt in info[map_name][lane]
])[:, :2]
keys = [(map_name, lane, pti) for lane in info[map_name]
for pti, pt in enumerate(info[map_name][lane])]
tree = KDTree(data)
trees[map_name] = {"tree": tree, "keys": keys}
print("done!")
return info, trees
def plot_map_info(self, ax, agent_pos, nusc_map, text_box=True):
closest_lane_id = nusc_map.get_closest_lane(agent_pos[0],
agent_pos[1],
radius=2)
closest_lane_record = nusc_map.get_lane(closest_lane_id)
closest_lane_poses = np.array(
arcline_path_utils.discretize_lane(closest_lane_record,
resolution_meters=1))
incoming_lane_ids = nusc_map.get_incoming_lane_ids(closest_lane_id)
incoming_lane_data = []
for incoming_lane_id in incoming_lane_ids:
i_record = nusc_map.get_lane(incoming_lane_id)
i_poses = np.array(
arcline_path_utils.discretize_lane(i_record,
resolution_meters=1))
incoming_lane_data.append({'record': i_record, 'poses': i_poses})
outgoing_lane_ids = nusc_map.get_outgoing_lane_ids(closest_lane_id)
outgoing_lane_data = []
for outgoing_lane_id in outgoing_lane_ids:
o_record = nusc_map.get_lane(outgoing_lane_id)
o_poses = np.array(
arcline_path_utils.discretize_lane(o_record,
resolution_meters=1))
outgoing_lane_data.append({'record': o_record, 'poses': o_poses})
map_info = {
'closest_lane': {
'record': closest_lane_record,
'poses': closest_lane_poses
},
'incoming_lanes': incoming_lane_data,
'outgoing_lanes': outgoing_lane_data
}
for k, v in viewitems(map_info):
if k == 'stop_line':
for d in v:
bd = d['bounding_box']
center = [(bd[0] + bd[2]) / 2, (bd[1] + bd[3]) / 2]
if text_box:
self.plot_text_box(
ax, 'detected_' + d['record']['stop_line_type'],
center, 'blue')
elif k == 'closest_lane':
p = np.array(v['poses'])
ax.plot(p[:, 0],
p[:, 1],
linestyle="-.",
linewidth=2,
color='yellow')
elif k == 'incoming_lanes':
for d in v:
p = np.array(d['poses'])
ax.plot(p[:, 0],
p[:, 1],
linestyle="-.",
linewidth=2,
color='brown')
elif k == 'outgoing_lanes':
for d in v:
p = np.array(d['poses'])
ax.plot(p[:, 0],
p[:, 1],
linestyle="-.",
linewidth=2,
color='white')
else:
raise ValueError(f'info type {k} not supported')