fixed iterator typo: now iterating from occupied.begin to occupied.end
Merge branch 'groovy-devel' of https://github.com/ros-planning/moveit_ros into groovy-devel