We should use OMPL's reed-shepp paths to periodically sample every N iterations (we need to figure out what N is) for the shortest path between the goal and the current pose. if that path is valid (e.g. collision free) then we can stop searching, append this path to the searched path and return for smoothing without needing to continue to search.
This optimization speeds things up alot according to the paper
We should use OMPL's reed-shepp paths to periodically sample every
Niterations (we need to figure out whatNis) for the shortest path between the goal and the current pose. if that path is valid (e.g. collision free) then we can stop searching, append this path to the searched path and return for smoothing without needing to continue to search.This optimization speeds things up alot according to the paper