File size: 4,138 Bytes
81c59cd
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
"""Render four 3.5-second demonstrations as an H.264 video."""
import subprocess
from pathlib import Path
from PIL import Image, ImageDraw, ImageFont
from servo import DEMOS, clearance, forward, interpolate, plan

SIZE = (960, 540)
FPS = 24
INK, GREEN, BG = "#102923", "#087b64", "#f3f5ef"
TITLES = ["Carry across the fixture", "Reach the high shelf",
          "Transfer between stations", "Exit a narrow pocket"]

def font(size, bold=False):
    for directory, regular, heavy in [
        (Path("C:/Windows/Fonts"), "segoeui.ttf", "segoeuib.ttf"),
        (Path("/usr/share/fonts/truetype/dejavu"), "DejaVuSans.ttf", "DejaVuSans-Bold.ttf"),
    ]:
        path = directory / (heavy if bold else regular)
        if path.exists():
            return ImageFont.truetype(str(path), size)
    return ImageFont.load_default(size=size)

title, label, body, hero = font(36,True), font(15,True), font(19), font(48,True)

def xy(p):
    return 335+100*p[0], 293-100*p[1]

def disk(draw, p, r, **style):
    x,y = p
    draw.ellipse((x-r,y-r,x+r,y+r), **style)

def frame(case_id, case, route, progress):
    start, target, obstacle = case
    image = Image.new("RGB", SIZE, BG)
    d = ImageDraw.Draw(image)
    d.text((36, 23), "NEXT SERVO   /   MOTION PLANNING", font=label, fill=GREEN)
    d.text((36, 49), TITLES[case_id], font=title, fill=INK)
    d.rounded_rectangle((36, 105, 634, 510), radius=18, fill="#eaf0e9", outline="#ccd9cf", width=2)
    for x in range(85, 620, 25):
        d.line((x, 127, x, 488), fill="#d9e3d9")
    for y in range(127, 499, 25):
        d.line((60, y, 610, y), fill="#d9e3d9")
    x,y,r = obstacle
    disk(d, xy((x,y)), 100*r, fill="#dea291", outline="#a74436", width=2)
    disk(d, xy(start), 8, outline="#59726b", width=2)
    disk(d, xy(target), 12, outline=GREEN, width=2)
    index = int(progress*(len(route)-1))
    q = interpolate(route[index], route[min(index+1,len(route)-1)],
                    progress*(len(route)-1)-index)
    elbow, tip = forward(q)
    trail = [xy(forward(a)[1]) for a in route[:index+1]]+[xy(tip)]
    d.line(trail, fill="#29947d", width=4)
    joints = [xy((0,0)), xy(elbow), xy(tip)]
    d.line(joints, fill=INK, width=8, joint="curve")
    for joint in joints[:2]:
        disk(d, joint, 9, fill=INK, outline="white", width=2)
    disk(d, joints[-1], 7, fill="#087b64", outline="white", width=2)
    d.text((667, 125), f"CASE {case_id+1:02d} / 04", font=label, fill=GREEN)
    d.text((667, 173), "Delivered" if progress == 1 else "Carrying", font=title, fill=INK)
    d.text((667, 253), "STRAIGHT JOINT MOVE", font=label, fill="#5e716a")
    d.text((667, 276), "Rejected by clearance check", font=body, fill=INK)
    d.text((667, 337), "PLANNER EDGE", font=label, fill="#5e716a")
    d.text((667, 360), f"{index} / {len(route)-1}", font=hero, fill=GREEN)
    d.line((667, 429, 924, 429), fill="#cdd9d0", width=2)
    for x, color, name in [(674, "#a74436", "Fixture"), (793, GREEN, "Payload")]:
        disk(d, (x, 462), 5, fill=color)
        d.text((x+12, 450), name, font=body, fill=INK)
    d.rectangle((36, 525, 924, 529), fill="#cad9cd")
    d.rectangle((36, 525, 36+888*progress, 529), fill=GREEN)
    return image

if __name__ == "__main__":
    output = Path("replay.mp4")
    partial = output.with_suffix(".partial.mp4")
    args = ["ffmpeg","-y","-loglevel","error","-f","rawvideo","-pix_fmt","rgb24",
            "-s","960x540","-r",str(FPS),"-i","pipe:0","-an","-c:v","libx264",
            "-preset","fast","-crf","20","-pix_fmt","yuv420p","-movflags","+faststart",str(partial)]
    with subprocess.Popen(args, stdin=subprocess.PIPE) as proc:
        for i, case in enumerate(DEMOS):
            route = plan(*case)
            if not route:
                raise RuntimeError(f"Unsolved demo {i+1}")
            for n in range(84):
                p = min(1, n/60)
                proc.stdin.write(frame(i,case,route,p).tobytes())
        proc.stdin.close()
        if proc.wait():
            raise RuntimeError("FFmpeg encoding failed")
    partial.replace(output)