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