def draw_vehicle_fill_tr(self, data_vehicle, id_sel, hcolor, tr):
# data_vehicle: (ndarray) t x y theta v length width tag_segment tag_lane id (dim = N x 10, width > length)
# id_sel: (ndarray) id of vehicles
# hcolor: (tuple) rgb color
# tr: (scalar) transparency 0 ~ 1
if ~isinstance(data_vehicle, np.ndarray):
data_vehicle = np.array(data_vehicle)
shape_data_vehicle = data_vehicle.shape
if len(shape_data_vehicle) == 1:
data_vehicle = np.reshape(data_vehicle, (1, -1))
num_vehicle = data_vehicle.shape[0]
for nidx_n in range(0, num_vehicle):
# Get vehicle-data
# structure: t x y theta v length width tag_segment tag_lane id (dim = 10, width > length)
data_vehicle_tmp = data_vehicle[nidx_n, :]
# Get polygon-points
pnts_vehicle_tmp = get_pnts_carshape(data_vehicle_tmp[1],
data_vehicle_tmp[2],
data_vehicle_tmp[3],
data_vehicle_tmp[6],
data_vehicle_tmp[5])
# Set (fill) color
if np.isin(data_vehicle_tmp[-1], id_sel):
cmap_vehicle = hcolor
else:
cmap_vehicle = get_rgb("Light Gray")
pnts_vehicle_pixel = self.convert2pixel(pnts_vehicle_tmp)
# Plot vehicle
for nidx_pnt in range(0, pnts_vehicle_pixel.shape[0]):
pnt_tmp = pnts_vehicle_pixel[nidx_pnt, :]
if nidx_pnt == 0:
self.ctx.move_to(pnt_tmp[0], pnt_tmp[1])
else:
self.ctx.line_to(pnt_tmp[0], pnt_tmp[1])
pnt_0_tmp = pnts_vehicle_pixel[0, :]
self.ctx.line_to(pnt_0_tmp[0], pnt_0_tmp[1])
# Plot (cairo)
self.ctx.set_line_width(1)
self.ctx.set_source_rgba(cmap_vehicle[0], cmap_vehicle[1],
cmap_vehicle[2], tr)
self.ctx.fill_preserve()
self.ctx.set_source_rgb(0, 0, 0)
self.ctx.set_line_width(1)
self.ctx.stroke()
def draw_trajectory_array_w_cost(self, traj_array, cost, idx_invalid,
cost_max, hlinewidth):
# traj_array: list of trajectory (ndarray, dim = N x 2)
# cost: (ndarray) cost of trajectory (dim = N)
# idx_invalid: (ndarray or list) invalid trajectory array
# cost_max: (scalar) cost_max (if 0, cost is scaled automatically)
# hlinewidth: (scalar) linewidth
if ~isinstance(cost, np.ndarray):
cost = np.array(cost)
cost = cost.reshape(-1)
num_traj = len(traj_array)
if len(idx_invalid) > 0:
idx_valid = np.setdiff1d(np.arange(0, num_traj), idx_invalid)
else:
idx_valid = np.arange(0, num_traj)
if len(idx_valid) > 0:
cost_valid = cost[idx_valid]
if cost_max == 0:
max_cost_valid, min_cost_valid = max(cost_valid), min(
cost_valid)
if abs(max_cost_valid - min_cost_valid) < float(1e-4):
max_cost_valid, min_cost_valid = 1.0, 0.0
else:
max_cost_valid, min_cost_valid = cost_max, 0.0
else:
max_cost_valid, min_cost_valid = 1.0, 0.0
# cmap = matplotlib.cm.get_cmap("cool")
cmap = matplotlib.cm.get_cmap("autumn")
for nidx_traj in range(0,
len(idx_invalid)): # Plot invalid trajectories
idx_sel = idx_invalid[nidx_traj]
traj_sel = traj_array[idx_sel]
self.draw_trajectory_tr(traj_sel[:, 0:2], hlinewidth,
get_rgb("Dark Slate Blue"), 0.95)
# self.draw_pnts_tr(traj_sel[:, 0:2], 3, get_rgb("Medium Slate Blue"), 0.5)
for nidx_traj in range(0, len(idx_valid)): # Plot valid trajectories
idx_sel = idx_valid[nidx_traj]
traj_sel = traj_array[idx_sel]
idx_tmp = (cost[idx_sel] - min_cost_valid) / (max_cost_valid -
min_cost_valid)
idx_tmp = min(max(idx_tmp, 0.0), 1.0)
cmap_1 = cmap(idx_tmp)
# cmap_1 = get_rgb("Orange")
self.draw_trajectory_tr(traj_sel[:, 0:2], hlinewidth, cmap_1[0:3],
0.95)
def draw_background(self, color_name):
# hcolor: (tuple) color
hcolor = get_rgb(color_name)
self.ctx.move_to(0, 0)
self.ctx.line_to(self.screen_size[0], 0)
self.ctx.line_to(self.screen_size[0], self.screen_size[1])
self.ctx.line_to(0, self.screen_size[1])
self.ctx.line_to(0, 0)
self.ctx.set_line_width(1)
self.ctx.set_source_rgb(hcolor[0], hcolor[1], hcolor[2])
self.ctx.fill()
def draw_track_sub(self, pnts_poly_track):
# pnts_poly_track: (list) points of track
# Convert to pixel space
pnts_pixel_track = []
num_lane_seg = 0 # number of lane-segment
for nidx_seg in range(0, len(pnts_poly_track)):
seg_sel = pnts_poly_track[nidx_seg]
pnts_pixel_seg = []
for nidx_lane in range(0, len(seg_sel)):
num_lane_seg = num_lane_seg + 1
pnts_tmp = seg_sel[nidx_lane]
pnts_conv_tmp = self.convert2pixel_sub(pnts_tmp)
pnts_pixel_seg.append(pnts_conv_tmp)
pnts_pixel_track.append(pnts_pixel_seg)
# Plot track
for nidx_seg in range(0, len(pnts_pixel_track)):
pnts_pixel_seg = pnts_pixel_track[nidx_seg]
# Plot lane-segment
for nidx_lane in range(0, len(pnts_pixel_seg)):
# Pnts on lane-segment
pnts_pixel_lane = pnts_pixel_seg[nidx_lane]
for nidx_pnt in range(0, pnts_pixel_lane.shape[0]):
pnt_tmp = pnts_pixel_lane[nidx_pnt, :]
if nidx_pnt == 0:
self.ctx.move_to(pnt_tmp[0], pnt_tmp[1])
else:
self.ctx.line_to(pnt_tmp[0], pnt_tmp[1])
pnt_0_tmp = pnts_pixel_lane[0, :]
self.ctx.line_to(pnt_0_tmp[0], pnt_0_tmp[1])
# Set (fill) color
cmap_lane = get_rgb("Dim Gray")
# Plot (cairo)
self.ctx.set_line_width(0.3)
self.ctx.set_source_rgb(cmap_lane[0], cmap_lane[1],
cmap_lane[2])
self.ctx.fill_preserve()
self.ctx.set_source_rgb(0, 0, 0)
self.ctx.set_line_width(0.3)
self.ctx.stroke()
def draw_track(self):
# pnts_poly_track: (list) points of track
if self.screen_mode == 0:
linewidth_outer, linewidth_inner = 1, 1
elif self.screen_mode == 1:
linewidth_outer, linewidth_inner = 2, 2
else:
linewidth_outer, linewidth_inner = 1, 1
self.update_pixel_track()
# Plot track (polygon)
for nidx_seg in range(0, len(self.pixel_poly_track)):
pixel_poly_seg = self.pixel_poly_track[nidx_seg]
# Plot lane-segment
for nidx_lane in range(0, len(pixel_poly_seg)):
idx_lane = len(pixel_poly_seg) - nidx_lane - 1
# Pnts on lane-segment
pixel_poly_lane = pixel_poly_seg[idx_lane]
for nidx_pnt in range(0, pixel_poly_lane.shape[0]):
pixel_tmp = pixel_poly_lane[nidx_pnt, :]
pixel_tmp = self.snapCoords(pixel_tmp[0], pixel_tmp[1])
if nidx_pnt == 0:
self.ctx.move_to(pixel_tmp[0], pixel_tmp[1])
else:
self.ctx.line_to(pixel_tmp[0], pixel_tmp[1])
pnt_0_tmp = pixel_poly_lane[0, :]
pnt_0_tmp = self.snapCoords(pnt_0_tmp[0], pnt_0_tmp[1])
self.ctx.line_to(pnt_0_tmp[0], pnt_0_tmp[1])
# Set (fill) color
cmap_lane = get_rgb("Dim Gray")
# Plot (cairo)
self.ctx.set_line_join(cairo.LINE_JOIN_ROUND)
self.ctx.set_source_rgb(cmap_lane[0], cmap_lane[1],
cmap_lane[2])
self.ctx.fill_preserve()
# self.ctx.set_source_rgb(0, 0, 0)
# self.ctx.set_line_width(linewidth_outer)
self.ctx.stroke()
# Plot track (outer)
for nidx_seg in range(0, len(self.pixel_outer_border_track)):
pixel_outer_seg = self.pixel_outer_border_track[nidx_seg]
for nidx_lane in range(0, len(pixel_outer_seg)):
pixel_outer_lane = pixel_outer_seg[nidx_lane]
for nidx_pnt in range(0, pixel_outer_lane.shape[0]):
pixel_tmp = pixel_outer_lane[nidx_pnt, :]
pixel_tmp = self.snapCoords(pixel_tmp[0], pixel_tmp[1])
if nidx_pnt == 0:
self.ctx.move_to(pixel_tmp[0], pixel_tmp[1])
else:
self.ctx.line_to(pixel_tmp[0], pixel_tmp[1])
# Set (fill) color
cmap_lane = get_rgb("Black")
# Plot (cairo)
self.ctx.set_line_width(linewidth_outer)
self.ctx.set_source_rgb(cmap_lane[0], cmap_lane[1],
cmap_lane[2])
self.ctx.stroke()
# Plot track (inner)
hcolor_inner = get_rgb("Silver")
for nidx_seg in range(0, len(self.pixel_inner_border_track)):
pixel_inner_seg = self.pixel_inner_border_track[nidx_seg]
for nidx_lane in range(0, len(pixel_inner_seg)):
pixel_inner_lane = pixel_inner_seg[nidx_lane]
for nidx_pnt in range(0, pixel_inner_lane.shape[0], 2):
pixel_cur_tmp = pixel_inner_lane[nidx_pnt, :]
pixel_cur_tmp = self.snapCoords(pixel_cur_tmp[0],
pixel_cur_tmp[1])
if (nidx_pnt + 1) <= (pixel_inner_lane.shape[0] - 1):
pixel_next_tmp = pixel_inner_lane[nidx_pnt + 1, :]
pixel_next_tmp = self.snapCoords(
pixel_next_tmp[0], pixel_next_tmp[1])
self.ctx.move_to(pixel_cur_tmp[0], pixel_cur_tmp[1])
self.ctx.line_to(pixel_next_tmp[0], pixel_next_tmp[1])
self.ctx.set_line_width(linewidth_inner)
self.ctx.set_source_rgb(hcolor_inner[0],
hcolor_inner[1],
hcolor_inner[2])
self.ctx.stroke()
else:
self.ctx.arc(pixel_cur_tmp[0], pixel_cur_tmp[1], 1, 0,
2 * math.pi)
self.ctx.set_source_rgb(hcolor_inner[0],
hcolor_inner[1],
hcolor_inner[2])
self.ctx.fill()