Class which calculates footprint cost

[shivaang12]

Feature request

Implement a class which is feeds on COSTMAP2D and provides functionality such as calculates cost of footprint at particular position.

Feature description

Implementation considerations

• I think it would be similar to these class from ROS Navigation
  https://github.com/ros-planning/navigation/blob/melodic-devel/base_local_planner/src/costmap_model.cpp

[SteveMacenski]

Looks about right. Or just rip that out of the current costmap collision checker and make it into a separate class that both the collision checker based on subscriptions and on direct costmap access use.