import plotutils
from mathutils import fit_gaussian, Gaussian

robot.enable()
move_to_laser()


RANGE = [-1.5, 1.5]
STEP = 0.025
Z_OFFSET = -1.0
LATENCY = 0.005

robot.set_motors_enabled(True)
current_positon = robot_x.getPosition()
robot_z.moveRel(Z_OFFSET)
ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)

d = ret.getReadable(0)

first_index = -1
last_index = -1
for i in range(len(d)):
    if not math.isnan(d[i]):
        if first_index<0:
            first_index = i  
        last_index = i  

if first_index == -1 or last_index < first_index:
    raise Exception("Invalid X detection")


center_index = (first_index + last_index)/2
center_positon = ret.getPositions(0)[center_index]
center_offset = center_positon-current_positon


print "X offset = ", center_offset