Changes sonar poses and getDistanceRightAndAngle() method.
Updated sonar return indexing