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

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