Fix missing include
Common Costmap Plugins (from other branch) + Refactor
Temp commit