[slotcar] Improve collision detection #36
Description
Right now the slotcar is requested to stop if the distance between its world pose and that of a non-static obstacles is less than a threshold. This is not very robust as two obstacles can have the same world pose but one with a larger geometry than the other. The slotcar will drive closer to the larger obstacle or worse might even collide. Hence, setting the threshold parameter is not very intuitive.
We should instead store AABBs of obstacles and then do a simple broadphase collision check between the robot AABB and that of the obstacle.