Create queue of programmatically set goals

[RaduMantu]

Adapt existing implementation from untracked NUC workspace for programmatic goals. Test the two solutions for queuing goals and chose one:

• follow_waypoints [1] <= preferred
• our implementation (found in the NUC workspace)

[1] http://wiki.ros.org/follow_waypoints