Add a costmap API for OccupancyGrid messages coming into the Simple Commander API

Basically have the basic API from https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp + adding global frame ID and timestamp with a constructor that takes in the OccupancyGrid.

Potentially even a python collision checker too when given a footprint from the footprint topic + this new CostmapPy class