Fix RRT rewire bug and code clean up

[AtsushiSakai]

Fix RRT rewire bug and code clean up #208

[coveralls]

Pull Request Test Coverage Report for Build 971

• 716 of 834 (85.85%) changed or added relevant lines in 20 files are covered.
• 34 unchanged lines in 6 files lost coverage.
• Overall coverage increased (+0.6%) to 90.012%

---

Changes Missing Coverage	Covered Lines	Changed/Added Lines	%
tests/test_grid_based_sweep_coverage_path_planner.py	0	1	0.0%
tests/test_rrt_star.py	0	1	0.0%
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py	31	33	93.94%
PathPlanning/LQRPlanner/LQRplanner.py	52	54	96.3%
tests/test_grid_map_lib.py	23	26	88.46%
PathPlanning/InformedRRTStar/informed_rrt_star.py	69	74	93.24%
PathPlanning/RRT/rrt.py	40	45	88.89%
PathPlanning/RRTStar/rrt_star.py	29	35	82.86%
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py	96	106	90.57%
PathPlanning/RRTDubins/rrt_dubins.py	74	85	87.06%

Files with Coverage Reduction	New Missed Lines	%
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py	1	80.42%
PathPlanning/LQRRRTStar/lqr_rrt_star.py	1	80.3%
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py	2	70.54%
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py	6	93.02%
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py	7	81.41%
SLAM/FastSLAM2/fast_slam2.py	17	89.66%

Totals
Change from base Build 927:	0.6%
Covered Lines:	6669
Relevant Lines:	7409

---

💛 - Coveralls

[AtsushiSakai]

close #214