Consider updating TrajectoryPlanner::getTrajectoryWithCost to return early if trajectory cost is over some threshold #3591
Description
Consider updating TrajectoryPlanner::getTrajectoryWithCost to return early if trajectory cost is over some threshold
Goal: reduce number of O (n*t) calls in collision evaluator
Testing:
- Outcome of best trajectory remains the same
- Ensure collision evaluator returns early if over threshold
- All path planning test cases pass