def main():
from argparser import parse_arguments
args = parse_arguments(ROOT_PATH)
print(ROOT_PATH)
print('loading map...')
projector = lanelet2.projection.UtmProjector(
lanelet2.io.Origin(args['lat_origin'], args['lon_origin']))
laneletmap = lanelet2.io.load(args['lanelet_map'], projector)
trafficRules = lanelet2.traffic_rules.create(
lanelet2.traffic_rules.Locations.Germany,
lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(laneletmap, trafficRules)
print('analyzing map...')
criticalAreas = map_analyzer.getAllCriticalAreas(
laneletmap, graph, args['critical_area_sim_thresh'])
laneletCaDict = map_analyzer.createLaneletCriticalAreaDict(criticalAreas)
print('loading trackfiles')
track_dictionary = dataset_reader.read_tracks(
args['interaction_trackfile'])
timestamp_min = 1e9
timestamp_max = 0
timestamp_delta_ms = 100
for key, track in iter(track_dictionary.items()):
timestamp_min = min(timestamp_min, track.time_stamp_ms_first)
timestamp_max = max(timestamp_max, track.time_stamp_ms_last)
timestamp = timestamp_min
patchesDict = dict()
textDict = dict()
visualize = args['visualize']
while True:
random_trackid = random.randint(1, len(track_dictionary))
if random_trackid in track_dictionary:
print('trackid %s is found' % (random_trackid))
break
random_trackid = 7
print(random_trackid)
if visualize:
fig, axes = plt.subplots(1, 1)
fig.canvas.set_window_title("Prediction Visualization")
drawing_utils.draw_fancy_lanelet_map(laneletmap, axes)
drawing_utils.draw_critical_areas(criticalAreas, axes)
title_text = fig.suptitle("")
fig2, axes2 = plt.subplots(1, 1)
# axes2.set_title("Data Visualization for random track: ",random_trackid)
fig2.canvas.set_window_title("Data Visualization for track %s " %
(random_trackid))
plt.xlim(track_dictionary[random_trackid].time_stamp_ms_first,
track_dictionary[random_trackid].time_stamp_ms_last)
#plt.ylim(-3.2, 3.2)
plt.plot([
track_dictionary[random_trackid].time_stamp_ms_first,
track_dictionary[random_trackid].time_stamp_ms_last
], [0, 0],
c='k')
colors = ['g', 'b', 'c', 'm', 'y', 'k', 'orange', 'aqua', 'lime']
markers = ['x', '+', 'v', '^', '1', '2', '3', '4', '5']
plt.ion()
plt.show()
activeObjects = dict()
while timestamp < timestamp_max:
if visualize:
start_time = time.time()
currentTracks = interaction_dataset_utilities.getVisibleTracks(
timestamp, track_dictionary)
possiblePathParams = lanelet2.routing.PossiblePathsParams()
possiblePathParams.includeShorterPaths = True
possiblePathParams.includeLaneChanges = False
for track in currentTracks:
currentMs = track.motion_states[timestamp]
if track.track_id not in activeObjects:
vehicleState = predictiontypes.Vehicle(objectId=track.track_id,
motionState=currentMs,
width=track.width,
length=track.length)
possiblePathsWithInfo = []
matchings = prediction_utilities.matchMotionState(
laneletmap,
currentMs) # match the car to several possible lanelets
for match in matchings: # for each start lanelet
possiblePathParams.routingCostLimit = lanelet2.geometry.approximatedLength2d(
match.lanelet) + 150
paths = map(
lambda x: predictiontypes.PathWithInformation(
laneletPath=x, caDict=laneletCaDict),
# caDict means conflict
graph.possiblePaths(match.lanelet, possiblePathParams))
possiblePathsWithInfo.extend(paths)
vehicleState.pathsWithInformation = possiblePathsWithInfo
activeObjects[track.track_id] = vehicleState
else:
vehicleState = activeObjects[track.track_id]
vehicleState.update(currentMs)
prediction_utilities.removeInactiveObjects(activeObjects, timestamp)
# TODO: continue here - calculate matching, build lanelet->critical area dictionary, associate track -> next ca, estimate state
if visualize:
plt.sca(axes)
# plt.axis('off')
# drawing_utils.draw_motion_states(track_dictionary, timestamp, axes, patchesDict, textDict)
drawing_utils.draw_vehicle_states(activeObjects, axes, patchesDict,
textDict)
prediction_utilities.cleanDrawingDicts(activeObjects, patchesDict,
textDict)
# fig.canvas.draw()
title_text.set_text("\nts = {}".format(timestamp))
if random_trackid in activeObjects.keys():
plt.sca(axes2)
plt.title('Arc distance for all paths')
basic_point = lanelet2.core.BasicPoint2d(
track_dictionary[random_trackid].motion_states[timestamp].
x, track_dictionary[random_trackid].
motion_states[timestamp].y)
in_area = 0
for i in range(len(criticalAreas.critical_areas)):
area_center = lanelet2.core.BasicPoint2d(
criticalAreas.critical_areas[i].x,
criticalAreas.critical_areas[i].y)
if lanelet2.geometry.distance(
basic_point, area_center
) <= criticalAreas.critical_areas[i].radius:
plt.scatter(timestamp,
track_dictionary[random_trackid].
motion_states[timestamp].psi_rad,
c='k',
s=10,
label='vehicle %s in critical area' %
random_trackid)
in_area = 1
break
if in_area == 0:
plt.scatter(timestamp,
track_dictionary[random_trackid].
motion_states[timestamp].psi_rad,
c='r',
s=1,
label='vehicle %s orientation' %
random_trackid)
for i in range(
len(activeObjects[random_trackid].
arcCoordinatesAlongPaths)): # for each path
arc_distance = activeObjects[
random_trackid].arcCoordinatesAlongPaths[i].distance
plt.scatter(timestamp,
arc_distance,
c=colors[i],
s=10,
label='path arcCoordinate distance %s' % (i),
marker=markers[i])
end_time = time.time()
if timestamp == track_dictionary[
random_trackid].time_stamp_ms_first:
plt.legend()
plt.pause(
max(0.001,
timestamp_delta_ms / 1000. - (end_time - start_time)))
timestamp += timestamp_delta_ms
if visualize:
plt.ioff()
return
def main():
args = argparser.parse_arguments(ROOT_PATH)
print('loading map')
projector = lanelet2.projection.UtmProjector(lanelet2.io.Origin(args['lat_origin'], args['lon_origin']))
laneletmap = lanelet2.io.load(args['lanelet_map'], projector)
trafficRules = lanelet2.traffic_rules.create(lanelet2.traffic_rules.Locations.Germany,
lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(laneletmap, trafficRules)
print('analyzing map')
criticalAreas = map_analyzer.getAllCriticalAreas(laneletmap, graph, args['critical_area_sim_thresh'])
laneletCaDict = map_analyzer.createLaneletCriticalAreaDict(criticalAreas)
print('loading trackfiles')
track_dictionary = dataset_reader.read_tracks(args['interaction_trackfile'])
while True:
randomTrackId = randint(1, 115)
if randomTrackId not in track_dictionary:
print('trackid %s not found' % (randomTrackId))
continue
track = track_dictionary[randomTrackId]
timestamp = track.time_stamp_ms_first
vehicleState = predictiontypes.Vehicle(objectId=track.track_id, motionState=track.motion_states[timestamp],
width=track.width, length=track.length)
matchings = prediction_utilities.matchMotionState(laneletmap, track.motion_states[timestamp])
pathsWithInfo = []
possiblePathParams = lanelet2.routing.PossiblePathsParams()
possiblePathParams.includeShorterPaths = True
possiblePathParams.includeLaneChanges = False
for match in matchings:
possiblePathParams.routingCostLimit = lanelet2.geometry.approximatedLength2d(match.lanelet) + 150
paths = map(lambda x: predictiontypes.PathWithInformation(laneletPath=x, caDict=laneletCaDict),
graph.possiblePaths(match.lanelet, possiblePathParams))
pathsWithInfo.extend(paths)
vehicleState.pathsWithInformation = pathsWithInfo
trackOrientations = []
timestamps = []
pathOrientations = [[] for j in range(len(pathsWithInfo))]
dt = 100
while timestamp <= track.time_stamp_ms_last:
timestamps.append(timestamp)
trackOrientations.append(track.motion_states[timestamp].psi_rad)
for i in range(len(pathsWithInfo)):
basicPoint = lanelet2.core.BasicPoint2d(track.motion_states[timestamp].x,
track.motion_states[timestamp].y)
arcCoordinates = lanelet2.geometry.toArcCoordinates(pathsWithInfo[i].centerline, basicPoint)
pathOrientations[i].append(pathsWithInfo[i].getOrientationAtArcLength(arcCoordinates.length))
timestamp += dt
fig, axes = plt.subplots(1, 1)
fig.canvas.set_window_title("Orientation of Track %s and its matching paths" % (randomTrackId))
plt.xlim(track.time_stamp_ms_first, track.time_stamp_ms_last)
plt.ylim(-math.pi, math.pi)
colors = ['g', 'b', 'c', 'm', 'y', 'k', 'orange', 'aqua', 'lime']
markers = ['x', '+', 'v', '^', '1', '2', '3', '4', '5']
plt.ion()
plt.show()
plt.sca(axes)
plt.scatter(timestamps, trackOrientations, c='r', s=2, label='Track orientation')
plt.plot(timestamps, trackOrientations, c='r')
for i in range(len(pathsWithInfo)):
plt.scatter(timestamps, pathOrientations[i], c=colors[i], label='Path number %s' % (i), marker=markers[i])
plt.plot(timestamps, pathOrientations[i], c=colors[i])
plt.legend()
plt.waitforbuttonpress()
plt.ioff()
def startProcessingPipeline(args):
print('loading map...')
projector = lanelet2.projection.UtmProjector(
lanelet2.io.Origin(args['lat_origin'], args['lon_origin']))
laneletmap = lanelet2.io.load(args['lanelet_map'], projector)
trafficRules = lanelet2.traffic_rules.create(
lanelet2.traffic_rules.Locations.Germany,
lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(laneletmap, trafficRules)
print('analyzing map...')
criticalAreas = map_analyzer.getAllCriticalAreas(
laneletmap, graph, args['critical_area_sim_thresh'])
laneletCaDict = map_analyzer.createLaneletCriticalAreaDict(criticalAreas)
print('loading trackfiles')
track_dictionary = dataset_reader.read_tracks(
args['interaction_trackfile'])
timestamp_min = 1e9
timestamp_max = 0
timestamp_delta_ms = 100
for key, track in iter(track_dictionary.items()):
timestamp_min = min(timestamp_min, track.time_stamp_ms_first)
timestamp_max = max(timestamp_max, track.time_stamp_ms_last)
timestamp = timestamp_min
patchesDict = dict()
textDict = dict()
visualize = args['visualize']
if visualize:
fig, axes = plt.subplots(1, 1)
# fig = plt.figure(1)
# axes = plt.subplot(2, 1,1)
fig.canvas.set_window_title("Prediction Visualization")
drawing_utils.draw_fancy_lanelet_map(laneletmap, axes)
drawing_utils.draw_critical_areas(criticalAreas, axes)
title_text = fig.suptitle("")
#axes2.set_title("Data Visualization for random track: ",random_trackid)
plt.ion()
plt.show()
activeObjects = dict()
while timestamp < timestamp_max:
if visualize:
start_time = time.time()
currentTracks = interaction_dataset_utilities.getVisibleTracks(
timestamp, track_dictionary)
possiblePathParams = lanelet2.routing.PossiblePathsParams()
possiblePathParams.includeShorterPaths = True
possiblePathParams.includeLaneChanges = False
for track in currentTracks:
currentMs = track.motion_states[timestamp]
if track.track_id not in activeObjects:
vehicleState = predictiontypes.Vehicle(objectId=track.track_id,
motionState=currentMs,
width=track.width,
length=track.length)
matchings = prediction_utilities.matchMotionState(
laneletmap,
currentMs) # match the car to several possible lanelets
possiblePathsWithInfo = []
for match in matchings: #for each start lanelet
possiblePathParams.routingCostLimit = lanelet2.geometry.approximatedLength2d(
match.lanelet) + 300
paths = map(
lambda x: predictiontypes.PathWithInformation(
laneletPath=x, caDict=laneletCaDict
), #caDict means conflict
graph.possiblePaths(match.lanelet, possiblePathParams))
possiblePathsWithInfo.extend(paths)
vehicleState.pathsWithInformation = possiblePathsWithInfo
activeObjects[track.track_id] = vehicleState
else:
vehicleState = activeObjects[track.track_id]
vehicleState.update(currentMs)
prediction_utilities.removeInactiveObjects(activeObjects, timestamp)
# TODO: continue here - calculate matching, build lanelet->critical area dictionary, associate track -> next ca, estimate state
if visualize:
plt.sca(axes)
#plt.axis('off')
#drawing_utils.draw_motion_states(track_dictionary, timestamp, axes, patchesDict, textDict)
drawing_utils.draw_vehicle_states(activeObjects, axes, patchesDict,
textDict)
prediction_utilities.cleanDrawingDicts(activeObjects, patchesDict,
textDict)
#fig.canvas.draw()
title_text.set_text("\nts = {}".format(timestamp))
end_time = time.time()
plt.pause(
max(0.001,
timestamp_delta_ms / 1000. - (end_time - start_time)))
# for i in activeObjects[1].pathsWithInformation[0].laneletSequence:
# print(i)
timestamp += timestamp_delta_ms
if visualize:
plt.ioff()
return
def main():
from argparser import parse_arguments
args = parse_arguments(ROOT_PATH)
print(ROOT_PATH)
print('loading map...')
map_name = (args['lanelet_map'].split('/')[-1]).split('.')[0]
trackfile = (args['interaction_trackfile'].split('/')[-1]).split('_')[-1]
projector = lanelet2.projection.UtmProjector(
lanelet2.io.Origin(args['lat_origin'], args['lon_origin']))
laneletmap = lanelet2.io.load(args['lanelet_map'], projector)
trafficRules = lanelet2.traffic_rules.create(
lanelet2.traffic_rules.Locations.Germany,
lanelet2.traffic_rules.Participants.Vehicle)
graph = lanelet2.routing.RoutingGraph(laneletmap, trafficRules)
print('analyzing map...')
criticalAreas = map_analyzer.getAllCriticalAreas(
laneletmap, graph, args['critical_area_sim_thresh'])
laneletCaDict = map_analyzer.createLaneletCriticalAreaDict(criticalAreas)
print('loading trackfiles')
track_dictionary = dataset_reader.read_tracks(
args['interaction_trackfile'])
timestamp_min = 1e9
timestamp_max = 0
timestamp_delta_ms = 100
for key, track in iter(track_dictionary.items()):
timestamp_min = min(timestamp_min, track.time_stamp_ms_first)
timestamp_max = max(timestamp_max, track.time_stamp_ms_last)
timestamp = timestamp_min
patchesDict = dict()
textDict = dict()
#parameter setting
visualize = args['visualize']
Deviation_alongarcCoordinate = 4
Time_difference_max = 3
Time_gap = 5
Default_gap = 40
Distance_difference_max = 20
PathLengthLimit = 500
if visualize:
fig, axes = plt.subplots(1, 1)
fig.canvas.set_window_title("Prediction Visualization in %s for %s" %
(map_name, trackfile))
drawing_utils.draw_fancy_lanelet_map(laneletmap, axes)
drawing_utils.draw_critical_areas(criticalAreas, axes)
title_text = fig.suptitle("")
fig2, axes2 = plt.subplots(1, 1)
fig2.canvas.set_window_title("Dependency Visualization in %s for %s" %
(map_name, trackfile))
G = nx.DiGraph()
plt.ion()
plt.show()
activeObjects = dict()
while timestamp < timestamp_max:
if visualize:
start_time = time.time()
currentTracks = interaction_dataset_utilities.getVisibleTracks(
timestamp, track_dictionary)
# possiblePathParams = lanelet2.routing.PossiblePathsParams()
# possiblePathParams.includeShorterPaths = True
# possiblePathParams.includeLaneChanges = False
for track in currentTracks:
currentMs = track.motion_states[timestamp]
if track.track_id not in activeObjects:
vehicleState = predictiontypes.Vehicle(
objectId=track.track_id,
motionState=currentMs,
width=track.width,
length=track.length,
timestamp_first=track.time_stamp_ms_first)
possiblePathsWithInfo = []
matchings = prediction_utilities.matchMotionState(
laneletmap,
currentMs) # match the car to several possible lanelets
for match in matchings: # for each start lanelet
#possiblePathParams.routingCostLimit = lanelet2.geometry.approximatedLength2d(match.lanelet) + 300 # a very important value. it means how far(meters) to consider
# paths = map(lambda x: predictiontypes.PathWithInformation(laneletPath=x, caDict=laneletCaDict),
# # caDict means conflict
# graph.possiblePaths(match.lanelet, possiblePathParams))
#possiblePathsWithInfo.extend(paths)
Pathset = possiblepath_calculate(
matching=match,
map_graph=graph,
pathLengthLimit=PathLengthLimit)
paths2 = map(lambda x: predictiontypes.PathWithInformation(
laneletPath=x, caDict=laneletCaDict),
Pathset) # caDict means conflict
possiblePathsWithInfo.extend(paths2)
vehicleState.pathsWithInformation = possiblePathsWithInfo
activeObjects[track.track_id] = vehicleState
else:
vehicleState = activeObjects[track.track_id]
vehicleState.update(currentMs)
prediction_utilities.removeInactiveObjects(activeObjects, timestamp)
# TODO: continue here - calculate matching, build lanelet->critical area dictionary, associate track -> next ca, estimate state
if visualize:
plt.sca(axes)
drawing_utils.draw_vehicle_states(activeObjects, axes, patchesDict,
textDict)
prediction_utilities.cleanDrawingDicts(activeObjects, patchesDict,
textDict)
title_text.set_text("\nts = {}".format(timestamp))
dependency_node, dependency_edges, rightofway_info, front_car_pairs, conflicting_car_pairs = dependency_calculate(
activeObjects, track_dictionary, timestamp, Default_gap,
Deviation_alongarcCoordinate, Time_gap, Time_difference_max,
Distance_difference_max)
hidden_intention = hidden_intentions_initialize(
activeObjects, front_car_pairs, conflicting_car_pairs,
Deviation_alongarcCoordinate, Distance_difference_max)
plt.sca(axes2)
G.clear()
axes2.cla()
G.add_nodes_from(dependency_node)
G.add_edges_from(dependency_edges)
nx.draw_circular(G, with_labels=True)
end_time = time.time()
plt.pause(
max(0.001,
timestamp_delta_ms / 1000. - (end_time - start_time)))
timestamp += timestamp_delta_ms
if visualize:
plt.ioff()
return