Checkin the current state of the AStar planner, still a WIP...
Merge branch 'master' of https://github.com/TheSentinel454/hubo_footstep_planner

Conflicts:
	FootstepPlanner/src/FootLocationNode.cpp