Merge ros-planning into my fork