Implemented all the methods in PathQueue

The PathQueue class can keep track of multiple paths for the Crowd