Constrained Smoother tuning

[AlexeyMerzlyakov]

During the benchmarking of Path Smoothers, it was discovered abnormal behavior of Constrained Smoother. This behavior appeared on some experiments with SmacHybridA* planner and Constrained Smoother, where the path smoothed by Constrained Smoother could have some “wobblings” or “oscillations” like depicted below:

The analysis is being performed and attached to current PR in a CS_path_oscillation_analysis_v7.pdf file.
As the results of analysis and during the numerous tests and benchmarks under different environments (on small and large maps, narrow- and wide-spaced) it was found and proved the optimal set of the residual coefficients causing Constrained Smoother to produce the rational path without oscillation effects:

Residual coefficient	Standard value change
w_smooth	15,000 → 2,000,000
w_cost	0.015
w_curve	30.0
w_dist	0.0

Thus, for best Constrained Smoother production experience is suggested to update default w_smooth value from 15K to 2M.

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu 20.04
Robotic platform tested on	TB3 Gazebo simulation, Path Smoothers benchmarking, colcon tests, lcov

---

Description of contribution in a few bullet points

• Updated defauly w_smooth value from 15K -> to 2M
• Tuned test_constrained_smoother for new w_smooth value:
  • testingObstacleAvoidance: cost avoidance improvement value was decreased, as expected from params ratio change
  • testingObstacleAvoidanceNearCusps: test seems to be tuned for the 15K w_smooth value - leaved untouched
  • testingDownsamplingUpsampling: slight smoothness improvement change
  • testingStartGoalOrientations: slight change of smoothness improvement in normal orientation, ~2x change in smoothness improvement and angles in reversed path orientation
• Fixed dist calculation formula in test_constrained_smoother causing incorrect exponent factor calculation, which causes overflows in costs making incorrect default costmap for the test (although, having no effect on final test results)
• Fixed incorrect integer ROS-parameter setting while it is required floating-point one for some cases in test_constrained_smoother test. This issue leaded to w_smooth coefficient was not being updated in these cases.

Description of documentation updates required from your changes

• w_smooth default value should be updated at the CS configuration guide page, if this change to be agreed and accepted

---

Future work that may be required in bullet points

• (optionally and might be not necessary) Re-work test_constrained_smoother -> testingObstacleAvoidanceNearCusps testcase for new w_smooth = 2M value

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[mergify]

@AlexeyMerzlyakov, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

Docs update to match changes on the website?

[AlexeyMerzlyakov]

Sure, if you are OK with w_smooth 2M new value (and also with this change in general), I'll submit a new doc update PR.

[SteveMacenski]

Please do

[AlexeyMerzlyakov]

Done: ros-navigation/docs.nav2.org#385. Seems to be mentioned only on two places, there is no tutorials or something else needed to be updated.