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

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