Update LRTAstar.cpp