Node duplication issue at the connection node of dubinShot and regular search

[yslenjoy]

path_planner/src/algorithm.cpp

Line 493 in 524a489

dubinsNodes[i].setPred(&start);

This line is at the condition that i equals to 0, it sets dubinsNodes[0]'s predecessor as start node. However, checking dubinsNodes[0] == start would get true, which means the first dubin node will equal to is predecessor, and it brings node duplication at the connection of dubinShot and regular search.

(I am using these node results for velocity profile generation, thus duplicated node definitely affect the angle calculation)

[karlkurzer]

Does this fix the issue?

path_planner/src/algorithm.cpp

Line 480 in a76a02a

x += Constants::dubinsStepSize;