This library will be written into the path_planning_handler on the agent singleton. Will be affected by the grid paradigm issue #9 .
