Add code to generate a informative dubins path
Fix problem with the generation of points in the dubins path