Ejemplo n.º 1
0
def world_loop(opts_dict):
    params = {
            'spawn': 15,
            'weather': 'clear_noon',
            'n_vehicles': 0
            }

    with cu.CarlaWrapper(TOWN, cu.VEHICLE_NAME, PORT) as env:
        env.init(**params)
        agent = RoamingAgent(env._player, False, opts_dict)

        # Hack: fill up controller experience.
        for _ in range(30):
            env.tick()
            env.apply_control(agent.run_step()[0])

        for _ in tqdm.tqdm(range(125)):
            env.tick()

            observations = env.get_observations()
            inputs = cu.get_inputs(observations)

            debug = dict()
            control, command = agent.run_step(inputs, debug_info=debug)
            env.apply_control(control)

            observations.update({'control': control, 'command': command})

            processed = cu.process(observations)

            yield debug

            bzu.show_image('rgb', processed['rgb'])
            bzu.show_image('birdview', cu.visualize_birdview(processed['birdview']))
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
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)