Obstacle Detection

The obstacle detection code can be found in obstacle_detector.py. Currently, it takes in a LaserScan message (from an RPLIDAR A2, for example), transforms the points into the map frame, clusters the points into groups that are approximately the size of the PVC pipes on top of an obstacle roomba. It then fits each of these clusters to a circle using a nonlinear least squares approach. The obstacles and their locations are then published on /obstacles.