Python script to add an OSD to a video based on ArduPilotMega log file

apm_osd.py

#!/usr/bin/env python
# add OSD from ArduPilotMega log file to a video

import sys
from path import path
from datetime import datetime, timedelta
from PIL import Image, ImageDraw
from math import sin, cos, radians

# STEP 1: use mplayer to extract jpgs and wav
# mplayer ../Videos/SUNP2002.AVI -nosound -vo jpeg:outdir=img -ss 00:06:00
# mplayer -vo null -vc null -ao pcm:fast:waveheader:file=temp.wav ../Videos/SUNP2002.AVI -ss 00:06:00


# STEP 2: synchronise the video and APM log file
# avi_gps_delta = timedelta(hours=1,minutes=6,seconds=9) # estimate based on filming clock on gps
avi_gps_delta = timedelta(hours=1,minutes=6,seconds=6) # estimate based on landing from gps log and video
movie_start = datetime(2011,4,11,11,33,21)-avi_gps_delta # local time
frame_offset = 8 # get seconds to click over at the same frame in the video
# frame_rate = 30.0 # stated
# frame_rate = 31.0 # mplayer
frame_rate = 28.48 # elapsed time from timestamps divided by number of frames


frame_jpgs = sorted(path("img").files("*.jpg"))
frames = [(movie_start+timedelta(microseconds=1000000 * int(round(4*(i-frame_offset)/frame_rate))/4.0),f) for (i,f) in enumerate(frame_jpgs)]

if "--test" in sys.argv:
    frames = frames[:int(frame_rate*2)]

log_dict = {}
att_dict = {}
ts = None
mode = "MOD:Manual"
for l in path("../Logs/11-04-11_19.log").lines():
    if l[:4]=="GPS:":
        log = map(lambda f:float(f.replace(",","")),l.strip().split(" ")[1:])
        ts = datetime(2011,4,11,10,0,0)+timedelta(microseconds=1000*log[0])
        log_dict[ts] = log
    if l[:4]=="ATT:" and ts is not None:
        att_dict[ts] = map(lambda f:float(f.replace(",","")),l.strip().split(" ")[1:])+[mode,]
    if l[:4]=="MOD:":
        mode = l.strip()

colour = 0xffffff
bg = 0x000000
center = 360

def text(draw, x,y, s):
    draw.text( (x-1, y-1), s, fill=bg )
    draw.text( (x-1, y+1), s, fill=bg )
    draw.text( (x+1, y-1), s, fill=bg )
    draw.text( (x+1, y+1), s, fill=bg )
    draw.text( (x, y), s, fill=colour )

min_alt = None
max_alt = None
for (i,(ts,jpg)) in enumerate(frames):
    print i,"/",len(frames)
    gps = log_dict[ts]
    alt, speed, heading = gps[6:9]

    # fudge alt so it's above ground level
    alt -= 47
    
    img = Image.open(jpg)
    d = ImageDraw.Draw(img)

    # time
    text(d, 630, 440, ts.strftime("%H:%M:%S"))
    
    # speed
    text(d, 10, 10, str(int(speed))+" km/h")

    # heading
    text(d, center-10, 10, str(int(heading))+chr(176))
    d.polygon([(center-5,25),(center,30),(center+5,25)], outline=colour)
    d.line([(center-100,35),(center+100,35)], fill=colour)
    for c,b in [("W",-90),("N",0),("E",90),("S",180),("W",270),("N",360),("E",450)]:
        x = center + (b - int(heading))
        if x > (center-100) and x < (center+100):
            text(d,x,40,c)

    # alt
    d.line([(10,35),(10,470)], fill=colour)
    if min_alt is not None:
        min_alt = min(min_alt,alt)
        y = 470 - 2*int(min_alt)
        d.polygon([(20,y),(15,y),(20,y+5)], outline=colour)
        text(d, 25, y+5, str(int(min_alt))+" m")
    else:
        min_alt = alt
    if max_alt is not None:
        max_alt = max(max_alt,alt)
        y = 470 - 2*int(max_alt)
        d.polygon([(20,y-5),(15,y),(20,y)], outline=colour)
        text(d, 25, y-10, str(int(max_alt))+" m")
    else:
        max_alt = alt
    y = 470 - 2*int(alt)
    text(d, 25, y-5, str(int(alt))+" m")
    d.polygon([(20,y-5),(15,y),(20,y+5)], outline=colour)
        
    # horizon
    roll, pitch, yaw = map(lambda v:int(v/100.0), att_dict[ts][:3])
    
    y = 240+90*sin(radians(pitch))
    text(d,center,y-25,str(roll)+chr(176))
    text(d,center,y+15,str(pitch)+chr(176))
    d.line( [(center-100*cos(radians(roll)), y+100*sin(radians(roll))),
             (center+100*cos(radians(roll)), y-100*sin(radians(roll)))],
            fill=colour )
    d.line( [(center,230),(center,250)], fill=colour )
    d.line( [(center-10,240),(center+10,240)], fill=colour )

    #text(d,center,260,str(yaw)+chr(176))

    # mode
    mode = att_dict[ts][3]
    text(d,600,10,mode)
    
    del d
    img.save(jpg.replace("img/","out/"), "JPEG")


# LASTLY: use mencoder to convert frames back to a video (fps derived from audio duration / number of frames)
# mencoder mf://out/*.jpg -mf fps=28.23 -o osd.avi -ovc lavc -lavcopts vcodec=mpeg4:mbd=1:vbitrate=2800 -oac mp3lame -audiofile temp.wav