Fixed issue causing trajectory planner returning false to isGoalReached even when it's control thread finishes executing
merged