inital differential drive trajectory controller - looks pretty good. needs a bit more code improvent, dynamic reconfigure and stricter obstacle testing