all works with modified pr2_navigation.