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

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