Wrong time values on speed_profile on a particular case of speeddown

[vlcfaria]

When running speed_profile first the time necessary for the first curve is added here:

AcceleratedDubins.jl/src/AcceleratedDubins.jl

Lines 758 to 761 in f2b89c9

	push_vals(time, speeds[1])
	time += lengths[1] / speeds[1]
	push_vals(time, speeds[1])

However, if the speeddown to the latter speed is not possible and therefore the speed must be reduced in the first curve, extra time regarding the first curve is added anyway, without deducting the previous added time:

AcceleratedDubins.jl/src/AcceleratedDubins.jl

Lines 870 to 872 in f2b89c9

	elseif sp_down_finish_len > lengths[2] && sp_down_finish_len < (lengths[1] + lengths[2])
	time += (lengths[1] + lengths[2] - sp_down_finish_len) / speeds[1]
	push_vals(time, speeds[1])

I believe the correct approach here would first deduct the previously added time and popping both vectors, then adding the correct one:

time -= lengths[1] / speeds[1]
pop!(times)
pop!(speed_at_time)