Fix Reeds-Shepp planner

[hcarlsso]

Fixes the Reeds-Shepp planner and adds a unit test.

[petercorke]

Thanks for your interest in the Toolbox, and for your PR. Accepted (with very minor change) into future branch.

[petercorke]

Hi @hcarlsso. I merged your code but the extra unit test you made fails for me. I get the turn sequence RLR whereas your test says the result should be LRL. Theoretically both sequences give the same result. The output of calc_paths that I get is:

[_Path: L=3.1, [L=1, R=-1, L=1], 35 points on path, _Path: L=3.1, [R=1, L=-1, R=1], 33 points on path]
special variables
function variables
0 =
_Path: L=3.1, [L=1, R=-1, L=1], 35 points on path
1 =
_Path: L=3.1, [R=1, L=-1, R=1], 33 points on path
len() =
2

which shows both paths have a length of 3.1 (pi). I suspect some very small arithmetic error makes my computer (MacOS+arm) choose RLR and your computer chooses LRL.

Can you please confirm your results and tell me what your platform is.

Bottom line, this is probably not a great example to use for a test given two possible answers.