The planner should be able to collision check a map

[bgill92]

Use Case

The path planner is trying to find a valid path from the start to the goal and needs to check if a pose will lead to a collision

Acceptance Criteria

GIVEN a certain robot footprint, map, and pose
WHEN checking to see if the footprint will lead to a collision in a map
THEN a function will return true or false