Updating planner_test
Updating planner_test + README
Merge branch 'master' of https://github.com/velveteenrobot/fydp_demo