Added Lidar_control pkg
Updated pkg xml & CMake
Fixed interpolation