Smooth nearest diagram algorithm prepared for ROS. Modified implementation of SND driver from Player project.