Adding Topological Navigation Node

This pull request adds the topological Navigation node