Raspberry Pi Car Cam - GPS Data & Map
Ever since I made my car cam which recorded video and overlaid OBD data, I’ve continued to work on the project and in particular on overlaying GPS data. I wanted to create something which was similar to the mini map you get on car racing games which shows you where the car is on the track as its going round.
Im not at a race track often, so I took Mrs O’Hanlon’s fiesta for a spin around Stourbridge ring road, its got speed restrictions and its very busy but it does at least go round and round!
This has been a pretty intense project, probably the most complex I’ve done with the raspberry pi and has involved several steps:
- I needed a better way of syncing data gathered by the Pi with video taken on the camera, with my first OBD overlay it was ‘VERY’ difficult to get it lined up. To acheive this I made a custom version of Raspivid to allow me to read, in real time, what frame it was on
- I had to create a python class to control raspivid
- I had to set up a GPS receiver and create a python class to control it
I then wrote a program which would start up my custom raspivid program, read the frame number alongside the GPS data and create a separate movie which would be the map and the data. I could then overlay the data video on top of the camera video and bingo, mini map on the side of my car video.
The data movie is created by producing an animation of single images. While raspivid is running my program continually reads the current frame count, the GPS data and then using PIL (python imaging library) creates and image of the map and some GPS data (lat, long, speed) for that frame.
The map is made by converting GPS co-ordinates to XY positions using a Mercator projection and then each frame is drawn by creating lines between all the sets of XY positions captured since the video started.
Where there are gaps between the frames, I use symbolic links to reference the last frame that was created, so I end up with a data frame (image) for every frame in the car video, this is frame 4993:
I then used mencoder to join all the frames together, in exactly the same way you would for a time lapse video.
My rig was pretty basic, I ran a simple program at boot which started the GPS service, lit an led and waited for a button to be pressed, when the button was pressed it started my program, which ran until it timed out and then the program shut the pi down. Plenty of gaffer tape was also used to stick it to the dashboard!
Finally once I had 2 videos, one from the camera and one from the data, I used Microsoft Expression (its a free and pretty effective encoder for Windows) to overlay the data onto of the video.
You will find all my code at https://github.com/martinohanlon/vidGPSOverlay, although please don’t expect it to just run, there are dependencies for my custom raspivid program as well as the GPS service, so you may need to give it a bit of love to get it going!
The code:
from PIL import Image, ImageFont, ImageDraw
from RaspividController import *
from GPSController import *
import time
import datetime
DATAFRAME_WIDTH = 500
DATAFRAME_HEIGHT = 300
MAP_WIDTH = 300
MAP_HEIGHT = 300
MAX_SCALE = 1
FONT_PATH = "/usr/share/fonts/truetype/freefont/FreeMono.ttf"
VIDEOTIME = 600000
def drawPoint(imagedraw,x,y,width,colour):
imagedraw.ellipse((x-(width/2),y-(width/2),x+(width/2),y+(width/2)), colour)
class DataDrawer():
def __init__(self, imagesFolder):
#setup variables
self.imagesFolder = imagesFolder
self.imageSize = (300,300)
self.minX = 99999999999
self.maxX = -99999999999
self.minY = 99999999999
self.maxY = -99999999999
self.lastFrameNo = 0
self.lastLat = 0
self.lastLon = 0
self.xyPositions = []
self.mapScale = 1
self.padX = 0
self.padY = 0
#load font
self.font = ImageFont.truetype(FONT_PATH, 14)
#create first frame
self.newDataFrame(1, 0, 0 ,0)
def newDataFrame(self, frameNo, speed, lat, lon):
#check to make sure the frame has moved on since last time
if frameNo > self.lastFrameNo:
#create sumbolic links between last frame and this frame
for missingFrameNo in range(self.lastFrameNo+1, frameNo):
os.symlink(self.imagesFolder + "/" + "{0:06d}".format(self.lastFrameNo) + ".jpg",
self.imagesFolder + "/" + "{0:06d}".format(missingFrameNo) + ".jpg")
#create new image
frame = Image.new("RGBA", (DATAFRAME_WIDTH, DATAFRAME_HEIGHT))
frameDraw = ImageDraw.Draw(frame)
#data
frameDraw.text((315,10)," Speed " + str(round(speed,2)),font=self.font)
frameDraw.text((315,50)," Latitude " + str(lat),font=self.font)
frameDraw.text((315,90),"Longitude " + str(lon),font=self.font)
#map
#only create map if we have a GPS fix
if lat != 0 and lon != 0:
#only add a new set of coords if the lat and lon have changed
if self.lastLat != lat or self.lastLon != lon:
#get x & y coords
x,y = GpsUtils.latLongToXY(lat, lon)
#print "x " + str(x)
#print "y " + str(y)
#add x,y to list
self.xyPositions.append([x,y])
#update mins and maxs
if x < self.minX: self.minX = x
if x > self.maxX: self.maxX = x
if y < self.minY: self.minY = y
if y > self.maxY: self.maxY = y
#persist lat and lon
self.lastLat = lat
self.lastLon = lon
#calculate scale
diffX = self.maxX - self.minX
#print "diffX " + str(diffX)
diffY = self.maxY - self.minY
#print "diffY " + str(diffY)
if diffX > diffY:
if diffX != 0: self.mapScale = MAP_WIDTH / float(diffX)
else: self.mapScale = 1
else:
if diffY != 0: self.mapScale = MAP_HEIGHT / float(diffY)
else: self.mapScale = 1
#print "mapScale " + str(self.mapScale)
#set max scale
if self.mapScale > MAX_SCALE: self.mapScale = MAX_SCALE
#re-calculate padding
self.padX = int((MAP_WIDTH - (diffX * self.mapScale)) / 2)
self.padY = int((MAP_HEIGHT - (diffY * self.mapScale)) / 2)
#draw lines
for position in range(1, len(self.xyPositions)):
#draw line between previous position and this one
x1 = self.padX + abs((self.xyPositions[position-1][0] * self.mapScale) - (self.minX * self.mapScale))
y1 = self.padY + abs((self.xyPositions[position-1][1] * self.mapScale) - (self.maxY * self.mapScale))
x2 = self.padX + abs((self.xyPositions[position][0] * self.mapScale) - (self.minX * self.mapScale))
y2 = self.padY + abs((self.xyPositions[position][1] * self.mapScale) - (self.maxY * self.mapScale))
#print "coords - " + str(x1) + " " + str(y1) + " " + str(x2) + " " + str(y2)
frameDraw.line((x1, y1, x2, y2), fill="white", width=3)
#draw start and end point
if len(self.xyPositions) > 1:
# start
drawPoint(frameDraw, self.padX + abs((self.xyPositions[0][0] * self.mapScale) - (self.minX * self.mapScale)),self.padY + abs((self.xyPositions[0][1] * self.mapScale) - (self.maxY * self.mapScale)), 10, "red")
# end
drawPoint(frameDraw, self.padX + abs((self.xyPositions[len(self.xyPositions)-1][0] * self.mapScale) - (self.minX * self.mapScale)), self.padY + abs((self.xyPositions[len(self.xyPositions)-1][1] * self.mapScale) - (self.maxY * self.mapScale)), 10, "green")
#save image
frame.save(self.imagesFolder + "/" + "{0:06d}".format(frameNo) + ".jpg", "JPEG")
#update last frame
self.lastFrameNo = frameNo
# main program
if __name__ == "__main__":
try:
#create data folder
localtime = datetime.datetime.now()
foldername = "/home/pi/dev/cbb/vidGPSOverlay/data/" + str(localtime.year) + str(localtime.month) + str(localtime.day) + str(localtime.hour) + str(localtime.minute) + str(localtime.second)
if not os.path.exists(foldername): os.makedirs(foldername)
#start raspivid
print "starting raspivid controller"
vidcontrol = RaspiVidController(foldername+"/vid.h264", VIDEOTIME, False, ["-fps", "25", "-vf", "-hf"])
vidcontrol.start()
#start gps controller
print "starting gps contoller"
gpscontrol = GpsController()
gpscontrol.start()
#wait for controllers to startup and gps fix
time.sleep(3)
#create data file
datafile = open(foldername+"/data.csv", "w")
#create datadrawer object
datadrawer = DataDrawer(foldername)
#loop
while(vidcontrol.isAlive()):
#get framecount
framecount = vidcontrol.getFrameCount()
#wait for a bit as the gps data is a little behind + it allows the processor to have a rest!
time.sleep(0.1)
#get gps data
lat = gpscontrol.fix.latitude
lon = gpscontrol.fix.longitude
utc = gpscontrol.utc
speed = gpscontrol.fix.speed
speedMPH = speed * GpsUtils.MPS_TO_MPH
#write data
dataString = str(framecount) + "," + str(speedMPH) + "," + str(lat) + "," + str(lon) + "," + "\n"
datafile.write(dataString)
datadrawer.newDataFrame(int(framecount), speedMPH, lat, lon)
except KeyboardInterrupt:
print "Cancelled"
except:
print "Unexpected error:", sys.exc_info()[0], sys.exc_info()[1]
raise
finally:
#shutdown raspivid
vidcontrol.stopController()
vidcontrol.join()
print "stopped raspivid controllder"
#shutdown gps controller
gpscontrol.stopController()
gpscontrol.join()
print "stopped gps controllder"
#close data file
print "closing data file"
datafile.close()