Updating my fork with current head of ros-planning