Couple of minor bugfixes and added test library.
WIP
Merge remote-tracking branch 'fraser/routing_v2' into routing_v2