Text this: A Two-Stage Local Path Planning Algorithm Based on Sampling and Optimization Methods