DWA cost function toward goal

[KoenHan]

Signed-off-by: KoenHan sliver1998324@gmail.com

I changed it as follows:

• Changed the computation method of to_goal_cost.
  Implemented calc_to_goal_cost function more closely to the method proposed in the paper of DWA.

[coveralls]

Pull Request Test Coverage Report for Build 864

• 5 of 5 (100.0%) changed or added relevant lines in 1 file are covered.
• 36 unchanged lines in 3 files lost coverage.
• Overall coverage decreased (-0.2%) to 90.105%

---

Files with Coverage Reduction	New Missed Lines	%
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py	1	90.41%
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py	1	80.61%
PathPlanning/InformedRRTStar/informed_rrt_star.py	34	70.65%

Totals
Change from base Build 863:	-0.2%
Covered Lines:	6593
Relevant Lines:	7317

---

💛 - Coveralls

[AtsushiSakai]

@KoenHan Thank you for your PR. I changed the title of the PR.

[AtsushiSakai]

@KoenHan This function wrote by another guy in #126 . In the first version of it, I used a distance function for the goal cost. But he changed it to using angle towards the goal for the cost.

[AtsushiSakai]

@KoenHan It seems that you are right. His implementation is just calculating the angle between robot position vector and goal vector from the origin. I will merge it. Thank you for your great PR!!

[AtsushiSakai]

@KoenHan I cleaned up the code and refactoring. Please check the latest master code.

[KoenHan]

I understand.
Thank you, too!