Add support for non-circular robots

This adds support for parsing footprints, uses footprint points if available in the robot model, and rotates the footprint during collision checking, so that collision checks work properly.

This exposes an existing bug where map points that lie between footprint points are not collision-checked. This means that the robot can drive "onto" the end of a wall and become stuck on the wall in certain circumstances.