Smooth path even if goal pose is so much near to the robot

[CihatAltiparmak]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5422
Primary OS tested on	Ubuntu
Robotic platform tested on	Saha Robotic's Service Robots
Does this PR contain AI generated software?	Of course no
Was this PR description generated by AI software?	no

---

Description of contribution in a few bullet points

I have removed the limitation of pose number from smooth method, so that robot's plan can be smoothed

Description of documentation updates required from your changes

no

Description of how this change was tested

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

The path segments still need to be large enough to smooth, i.e. at least size 3, if not that size, it cannot be smoothed any more than it is. I think you just need to update 9 to 3 for this case. It was 9 due to copy+pasting from the SG Filter Smoother in the other plugin which does require length > 9 due to its formulation

[CihatAltiparmak]

It's reasonable. But we have to be careful about the cases which path has 2 pose and we does not set the orientation. Instead of removing the limitation, we can always set orientations first before smoothing? I've seen that when smoothing goes wrong, we always set orientation and thow an exception. How does it sounds to you? With these, we can fix SG Filter as well. two birds in one stone.

[SteveMacenski]

I think in #5424 we're discussing putting orientations into NavFn / Theta* itself, so that should handle that problem, no?

[CihatAltiparmak]

I've changed this part as 3 but IMO, it should be 1, it's because end - start + 1 gives you the pose number of segment, not end - start. additionaly, we should smooth the segments having 3 poses. end - start + 1 > 2 goes to end - start > 1 What do you think?

[SteveMacenski]

If we have a path of 1, we can't smooth it. If we have a path of 2, there's nothing to smooth, its always a straight line. Anything less than 3 is meaningless to 'smooth' because its a null operation.

[SteveMacenski]

I think this should be reverted.

[CihatAltiparmak]

Could you like to explain of why we have to use 2 - 1 as index ? I tried to search in detail and I still haven't understood.

[SteveMacenski]

The first point isn't planned, its typically just the robot's state. The i=1th point is the first truly planned point

[CihatAltiparmak]

Hmm, interesting, thank you. Just for sanity check, for ackermann car, would'nt it be a little bit problematic? for example car has 90 degree yaw angle but path goes along to 270 degree. In that case, the sign of the orientation can give rise to unfeasible control of car. Just question.

[SteveMacenski]

This smoother is only for holonomic planners

[SteveMacenski]

Otherwise LGTM!

[CihatAltiparmak]

I^ve applied your suggestions. Our next PR should be about sharing common smoothing util function with planners. Then time to the coup de grace goes to setting orientation at planning phase.