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

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