Path now shown in rviz