Switched custom controller away from ros-planning