CIMAR HLP DEV initial commit
Merge https://github.com/pemami4911/High-Level-Planner-CIMAR

Merging with remote repo