Parsing sonar data from a line distance sensor to reconstruct the surroundings of an underwater robot Sparus 2. The reconstructed 3D map was an input to a mission path planning algorithm. System was developed in a physics simulator Gazebo, reconstruction results were visualised in RViz and interoperability of those two with a data processing process was implemented using ROS. Successful field testing was performed on a real underwater robot from a boat on the coast of Spain.