search

usdot-fhwa-stol / carma-platform

CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication. Doxygen Source Code Documentation :
https://usdot-fhwa-stol.github.io/documentation/carma-platform/
392 stars 122 forks source link

yield_plugin is NOT stopping with comfortable decel when CP is on #2345

Open MishkaMN opened 3 months ago

MishkaMN commented 3 months ago

Summary

During verification testing data analysis of CARMA Systems 4.5.0 during the CDA Research-VRU project, it was discovered that the yield_plugin is generating trajectory with emergency decel rate even with CP turned on. Following are from graphs from two different runs, but describe same behavior:

example-deceleration-and-speed decel

Commit

951ca9a2bdd5212af032aedde0891bbe4b22f9a8

Expected Behavior

platform should decelerate with lower decel rate

Actual Behavior

see above

Steps to Reproduce the Actual Behavior

run cdasim with CP enabled and yield_plugin's minimum_safety_distance set to small number such as 1.0 meter

Related Work

No response

MishkaMN commented 2 months ago

One remaining issue here that I could identify was about the initial inputs that we are giving. Initial inputs specify: initial_access: 0. (https://github.com/usdot-fhwa-stol/carma-platform/blob/95f33725259f87a4da1268e35c67ed54dc997a93/yield_plugin/src/yield_plugin.cpp#L420) This condition arises from trying to achieve minimum jerk, which is 0 initial and end accel among other conditions.

However, the yield plugin is running at 10hz and everytime it is generating a new trajectory assuming 0 acceleration EVEN if it is already decelerating with certain non-zero accel. Therefore, the calculation is correct only on the first iteration, but gradually becomes more inaccurate. This is probably being displayed by the sudden cliff at the end where the accel suddenly drops to make the math work.

MishkaMN commented 2 months ago

Modification done under VRU use case was heavily influenced by stopping for potential obstacles whereas the original algorithm comes from more of "platooning" behavior, which is why the minimum jerk algorithm is picked rather than just assigning comfortable deceleration and stop. Therefore, any more fixes should keep both cases in mind.