Update goal to PointStamped for rviz