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

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