This is all the related worked has already been implemented in the CARLA Planning group in UW-Madison in 2021 and 2022.
Thanks to Yifan Yao and Binrui Yang in the past two years, I spend a great time with them.
*current page is incomplete, and I will finish it in the near future.