Improve stop_and_wait_plugin's curvature calculation when creating stopping trajectory

[MishkaMN]

Types of Issue

• [ ] Anomaly report (something appears to not work correctly)
• [X] Enhancement request (describe the enhancement being requested)
• [ ] Other (please ensure the description clarifies why the issue doesn’t fall into either of the above categories)

Descriptive summary

Currently stop_and_wait_plugin doesn't generate trajectory speed that account for sharp turns. For example, on the innovation drive driving west intersection and turning left at the west intersection to stop shortly after would generate too high speed for the turn. ILC however generates slow enough speed for this turn. Most of the normal routes we choose doesn't run into this issue, and this doesn't prevent normal operation of carma, but just adds robustness to the plugin's logic.

Carma version where this issue was discovered

carma-systems-4.2.0+ (develop)

Expected behavior

stop_and_wait_plugin should generate slow enough speed trajectory on turns

Actual behavior

See above.

Steps to reproduce the actual behavior

Select route with endpoint that is shortly after a curved lane following road section.

Related work

[MishkaMN]

This is not fixed so far I believe.