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

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