Created a program that can save the FootStepPlan message as a yaml file.
Created a yaml helper to automatically convert from the on-disk format
to the ROS msg format.

Also created a simple script that displays footstep plans
Now displaying the straight line path of the footstep plan
Added a new Roadmap type and the corresponding yaml helper functions.
Created a node that displays a roadmap in RVIZ