pure_pursuit code cleanup

1.Theoretically, the curvature should be 2.0pt.y/(pt.xpt.x + pt.y*pt.y). For the theory of pure pursuit controller and the derivation of the formula, please refer to "Integrated Mobile Robot Control" by Omead Amidi (CMU-RI-TR-90-17, Equation 3.10 on Page 21). 2. Rewrite a few functions so they are easier to understand. 3. Verified in both lgsim simulation and L5 test track with Lexus.