Plugin-ize costmap2d in the servers

[SteveMacenski]

Right now, costmap 2D is hardcoded into the controller and planner servers.

In the future, we may want other representations. While we could host the environmental representations in other processes, having low-latency, direct access to this data is necessary for algorithms with many querys and such that large messages don't block the thread which executing the callback.