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

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