[routing] Initial infrstructure for generalied routing
[routing] Added BFSQueue searching environment