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

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