[Smac planner] Looping motion primitive list in every getNeighbor call

[akchobby]

In the code snippet below , one can see the motion primitives list needs to be list iterated to find the direction change index but for a given set of motion primitives this will always be the same value. Can this be avoided ? or is there a particular reasoning behind it?

https://github.com/ros-planning/navigation2/blob/edfe5bf6bd785f92061f880535512cddc67da5ae/nav2_smac_planner/src/node_lattice.cpp#L476-L481

this call can be done once at the start and the value can be stored.

To reiterate what I have expressed below is a simple print that I have added to see if the direction change index ever change , in my test I don't see it change. (test done on the sample primitives ackermann with 0.5 turning radius)

[SteveMacenski]

This list is relatively small (e.g. 3-7) of the relative motion primitives of the current pose only. Its not all motion primitives of the entire lattice, but the motion primitives obtainable at the current heading.

But you are correct, that for the case that reversing is allowed, we should know that IDX at the parse time and that could be precomputed for each angular quantization and stored in the motion table. Is that something you'd be interested in doing? That would speed up the planner a bit, though I'm not sure it would be substantially faster given how small of an operation this is. To your own logging, that loop stops after 6 cycles. But hey, I'll never complain with slightly faster ;-)

[SteveMacenski]

@akchobby would you be open to contributing this?

[akchobby]

@akchobby would you be open to contributing this?

Yes I can take this up , shall do it towards the last week of December

[SteveMacenski]

Merging imminent