Finally got ranging working
Working on lidar plot.