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

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