Fixed replan requests not being issued
Stay in controlling mode when replanning from some custom pose by request of the local planner