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/
404 stars 123 forks source link

TIM scenario vehicle sped up to config_limit instead of slowing down when in geofence region #1270

Closed msmcconnell closed 3 years ago

msmcconnell commented 3 years ago

Types of Issue

Descriptive summary

While testing TIM at summit point, the vehicle sped up instead of slowing down in geofence area. Log inspection shows speed limit was set correctly, but possibly not extracted correctly in the planning component.

Log below shows the correct geofence speed limit of 25mph being set in carma_wm.

[0m1621358862.812332008 |  INFO | WMListenerWorker::mapUpdateCallback:159 | Geofence id99ce000d-3511-42f2-83a0-8b03f4f05348 requests update of size: 1[0m
[32m1621358862.812385762 | DEBUG | WMListenerWorker::newRegemUpdateHelper:226 | updateed llt id:24078, with digital speed limit of: 11.176in ms[0m
[0m1621358862.924289336 |  INFO | WMListenerWorker::mapUpdateCallback:194 | Finished Applying the Map Update with Geofence Id:99ce000d-3511-42f2-83a0-8b03f4f05348[0m
[0m1621358862.924370262 |  INFO | WMListener::mapUpdateCallback:79 | New Map Update Received. SeqNum: 3[0m
[0m1621358862.924385087 |  INFO | WMListenerWorker::mapUpdateCallback:108 | Map Update Being Evaluated. SeqNum: 3[0m
[0m1621358862.926711237 |  INFO | WMListenerWorker::mapUpdateCallback:140 | Processing Map Update with Geofence Id:4afe43e7-8ba2-4339-bd6a-b6d2c35b129c[0m
[32m1621358862.926749314 | DEBUG | WMListenerWorker::mapUpdateCallback:142 | Geofence id4afe43e7-8ba2-4339-bd6a-b6d2c35b129c requests removal of size: 0[0m
[0m1621358862.926764556 |  INFO | WMListenerWorker::mapUpdateCallback:159 | Geofence id4afe43e7-8ba2-4339-bd6a-b6d2c35b129c requests update of size: 1[0m
[0m1621358863.052567055 |  INFO | WMListenerWorker::mapUpdateCallback:194 | Finished Applying the Map Update with Geofence Id:4afe43e7-8ba2-4339-bd6a-b6d2c35b129c[0m
[0m1621358863.052746873 |  INFO | WMListener::mapUpdateCallback:79 | New Map Update Received. SeqNum: 4[0m
[0m1621358863.052764268 |  INFO | WMListenerWorker::mapUpdateCallback:108 | Map Update Being Evaluated. SeqNum: 4[0m
[0m1621358863.055202312 |  INFO | WMListenerWorker::mapUpdateCallback:140 | Processing Map Update with Geofence Id:c328d173-40e2-4ed7-bef4-28b96c42d5e6[0m
[32m1621358863.055235970 | DEBUG | WMListenerWorker::mapUpdateCallback:142 | Geofence idc328d173-40e2-4ed7-bef4-28b96c42d5e6 requests removal of size: 1[0m
[32m1621358863.055252671 | DEBUG | WMListenerWorker::mapUpdateCallback:149 | Regems found in lanelet: 6[0m
[32m1621358863.055293471 | DEBUG | WMListenerWorker::mapUpdateCallback:155 | Regems left in lanelet after removal: 5[0m
[0m1621358863.055307886 |  INFO | WMListenerWorker::mapUpdateCallback:159 | Geofence idc328d173-40e2-4ed7-bef4-28b96c42d5e6 requests update of size: 1[0m
[32m1621358863.055347589 | DEBUG | WMListenerWorker::newRegemUpdateHelper:226 | updateed llt id:24077, with digital speed limit of: 11.176in ms[0m
[0m1621358863.173309702 |  INFO | WMListenerWorker::mapUpdateCallback:194 | Finished Applying the Map Update with Geofence Id:c328d173-40e2-4ed7-bef4-28b96c42d5e6[0m
[0m1621358863.173418388 |  INFO | WMListener::mapUpdateCallback:79 | New Map Update Received. SeqNum: 5[0m
[0m1621358863.173435861 |  INFO | WMListenerWorker::mapUpdateCallback:108 | Map Update Being Evaluated. SeqNum: 5[0m

My initial impression is that the issue comes from the logic in CARMAUsTrafficControl for reading the speed limit from the digital speed limit regulatory element. Either no speed limit was found or the logic incorrectly changed the output to the config limit.

Carma version where this issue was discovered

release/zephyr

Expected behavior

Speed limit matches value set in Tahoe

Actual behavior

See description

Steps to reproduce the actual behavior

  1. Do this
  2. Then do this... Setup TIM scenario at TFHRC and run it with the zephyr branch.

    Related work

msmcconnell commented 3 years ago

Resolved by above PR