* fix dubins path length bug and clean up codes.
* fix line length CI error
* fix line length CI error
* fix line length CI error
* fix line length CI error
* fix line length CI error
* fix line length CI error
* fix line length CI error
* fix line length CI error
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up
* Test code clean up