Threaded waypoints

Now also does intermediate points and measures in HSV