def _paint(observations, control, diagnostic, debug, env, show=False):
import cv2
import numpy as np
WHITE = (255, 255, 255)
RED = (255, 0, 0)
CROP_SIZE = 192
X = 176
Y = 192 // 2
R = 2
birdview = cu.visualize_birdview(observations['birdview'])
birdview = crop_birdview(birdview)
if 'big_cam' in observations:
canvas = np.uint8(observations['big_cam']).copy()
rgb = np.uint8(observations['rgb']).copy()
else:
canvas = np.uint8(observations['rgb']).copy()
def _stick_together(a, b, axis=1):
if axis == 1:
h = min(a.shape[0], b.shape[0])
r1 = h / a.shape[0]
r2 = h / b.shape[0]
a = cv2.resize(a, (int(r1 * a.shape[1]), int(r1 * a.shape[0])))
b = cv2.resize(b, (int(r2 * b.shape[1]), int(r2 * b.shape[0])))
return np.concatenate([a, b], 1)
else:
h = min(a.shape[1], b.shape[1])
r1 = h / a.shape[1]
r2 = h / b.shape[1]
a = cv2.resize(a, (int(r1 * a.shape[1]), int(r1 * a.shape[0])))
b = cv2.resize(b, (int(r2 * b.shape[1]), int(r2 * b.shape[0])))
return np.concatenate([a, b], 0)
def _write(text, i, j, canvas=canvas, fontsize=0.4):
rows = [x * (canvas.shape[0] // 10) for x in range(10+1)]
cols = [x * (canvas.shape[1] // 9) for x in range(9+1)]
cv2.putText(
canvas, text, (cols[j], rows[i]),
cv2.FONT_HERSHEY_SIMPLEX, fontsize, WHITE, 1)
_command = {
1: 'LEFT',
2: 'RIGHT',
3: 'STRAIGHT',
4: 'FOLLOW',
}.get(observations['command'], '???')
if 'big_cam' in observations:
fontsize = 0.8
else:
fontsize = 0.4
_write('Command: ' + _command, 1, 0, fontsize=fontsize)
_write('Velocity: %.1f' % np.linalg.norm(observations['velocity']), 2, 0, fontsize=fontsize)
_write('Steer: %.2f' % control.steer, 4, 0, fontsize=fontsize)
_write('Throttle: %.2f' % control.throttle, 5, 0, fontsize=fontsize)
_write('Brake: %.1f' % control.brake, 6, 0, fontsize=fontsize)
_write('Collided: %s' % diagnostic['collided'], 1, 6, fontsize=fontsize)
_write('Invaded: %s' % diagnostic['invaded'], 2, 6, fontsize=fontsize)
_write('Lights Ran: %d/%d' % (env.traffic_tracker.total_lights_ran, env.traffic_tracker.total_lights), 3, 6, fontsize=fontsize)
_write('Goal: %.1f' % diagnostic['distance_to_goal'], 4, 6, fontsize=fontsize)
_write('Time: %d' % env._tick, 5, 6, fontsize=fontsize)
_write('FPS: %.2f' % (env._tick / (diagnostic['wall'])), 6, 6, fontsize=fontsize)
for x, y in debug.get('locations', []):
x = int(X - x / 2.0 * CROP_SIZE)
y = int(Y + y / 2.0 * CROP_SIZE)
S = R // 2
birdview[x-S:x+S+1,y-S:y+S+1] = RED
for x, y in debug.get('locations_world', []):
x = int(X - x * 4)
y = int(Y + y * 4)
S = R // 2
birdview[x-S:x+S+1,y-S:y+S+1] = RED
for x, y in debug.get('locations_birdview', []):
S = R // 2
birdview[x-S:x+S+1,y-S:y+S+1] = RED
for x, y in debug.get('locations_pixel', []):
S = R // 2
if 'big_cam' in observations:
rgb[y-S:y+S+1,x-S:x+S+1] = RED
else:
canvas[y-S:y+S+1,x-S:x+S+1] = RED
for x, y in debug.get('curve', []):
x = int(X - x * 4)
y = int(Y + y * 4)
try:
birdview[x,y] = [155, 0, 155]
except:
pass
if 'target' in debug:
x, y = debug['target'][:2]
x = int(X - x * 4)
y = int(Y + y * 4)
birdview[x-R:x+R+1,y-R:y+R+1] = [0, 155, 155]
ox, oy = observations['orientation']
rot = np.array([
[ox, oy],
[-oy, ox]])
u = observations['node'] - observations['position'][:2]
v = observations['next'] - observations['position'][:2]
u = rot.dot(u)
x, y = u
x = int(X - x * 4)
y = int(Y + y * 4)
v = rot.dot(v)
x, y = v
x = int(X - x * 4)
y = int(Y + y * 4)
if 'big_cam' in observations:
_write('Network input/output', 1, 0, canvas=rgb)
_write('Projected output', 1, 0, canvas=birdview)
full = _stick_together(rgb, birdview)
else:
full = _stick_together(canvas, birdview)
if 'image' in debug:
full = _stick_together(full, cu.visualize_predicted_birdview(debug['image'], 0.01))
if 'big_cam' in observations:
full = _stick_together(canvas, full, axis=0)
if show:
bzu.show_image('canvas', full)
bzu.add_to_video(full)
def _debug(observations, agent_debug):
import cv2
processed = cu.process(observations)
control = observations['control']
control = [control.steer, control.throttle, control.brake]
control = ' '.join(str('%.2f' % x).rjust(5, ' ') for x in control)
real_control = observations['real_control']
real_control = [
real_control.steer, real_control.throttle, real_control.brake
]
real_control = ' '.join(
str('%.2f' % x).rjust(5, ' ') for x in real_control)
canvas = np.uint8(observations['rgb']).copy()
rows = [x * (canvas.shape[0] // 10) for x in range(10 + 1)]
cols = [x * (canvas.shape[1] // 10) for x in range(10 + 1)]
WHITE = (255, 255, 255)
CROP_SIZE = 192
X = 176
Y = 192 // 2
R = 2
def _write(text, i, j):
cv2.putText(canvas, text, (cols[j], rows[i]), cv2.FONT_HERSHEY_SIMPLEX,
0.4, WHITE, 1)
_command = {
1: 'LEFT',
2: 'RIGHT',
3: 'STRAIGHT',
4: 'FOLLOW',
}.get(int(observations['command']), '???')
_write('Command: ' + _command, 1, 0)
_write('Velocity: %.1f' % np.linalg.norm(observations['velocity']), 2, 0)
_write('Real: %s' % control, -5, 0)
_write('Control: %s' % control, -4, 0)
r = 2
birdview = cu.visualize_birdview(crop_birdview(processed['birdview']))
def _dot(x, y, color):
x = int(x)
y = int(y)
birdview[176 - r - x:176 + r + 1 - x,
96 - r + y:96 + r + 1 + y] = color
_dot(0, 0, [255, 255, 255])
ox, oy = observations['orientation']
R = np.array([[ox, oy], [-oy, ox]])
u = np.array(agent_debug['waypoint']) - np.array(agent_debug['vehicle'])
u = R.dot(u[:2])
u = u * 4
_dot(u[0], u[1], [255, 255, 255])
def _stick_together(a, b):
h = min(a.shape[0], b.shape[0])
r1 = h / a.shape[0]
r2 = h / b.shape[0]
a = cv2.resize(a, (int(r1 * a.shape[1]), int(r1 * a.shape[0])))
b = cv2.resize(b, (int(r2 * b.shape[1]), int(r2 * b.shape[0])))
return np.concatenate([a, b], 1)
full = _stick_together(canvas, birdview)
bu.show_image('full', full)