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

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