Revert "Trimmed the branch to only the costmap_2d sub directory"

This reverts commit 86293b054ebf7cfe807a5ae815c25efe6f48792e.
Rebase from 'upstream'
Trimmed the branch to only the costmap_2d sub directory