The math to determine if a costmap point is within the rectangular safety bubble has been completed. Control for breach in bubble is finished.