Initial commit of global path stuff; not ROS-ified yet