82 lines
1.8 KiB
Python
82 lines
1.8 KiB
Python
import plotutils
|
|
from mathutils import fit_gaussian, Gaussian
|
|
|
|
|
|
d = robot.get_distance_to_pnt("pLaser")
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to laser: " + str(d))
|
|
|
|
if d>20:
|
|
raise Exception ("Should be near the laser position to perform the scan")
|
|
|
|
RANGE = [-2.0, 2.0] #[-1.5, 1.5]
|
|
STEP = 0.02
|
|
Z_OFFSET = 0 #-1.0
|
|
LATENCY = 0.025
|
|
BORDER_SIZE = 0.15
|
|
|
|
robot.enable()
|
|
robot.set_motors_enabled(True)
|
|
current_positon = robot_x.getPosition()
|
|
robot_z.moveRel(Z_OFFSET)
|
|
|
|
robot.setPolling(25)
|
|
try:
|
|
ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
|
|
finally:
|
|
robot.setPolling(DEFAULT_ROBOT_POLLING)
|
|
|
|
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 range")
|
|
|
|
|
|
remove = int(max(BORDER_SIZE, STEP) / STEP)
|
|
|
|
_range = [first_index+remove, last_index-remove]
|
|
if _range[1] <= _range[0]:
|
|
raise Exception("Invalid range: " + str(_range))
|
|
|
|
|
|
center_index = int((_range[0] + _range[1])/2)
|
|
center_positon = ret.getPositions(0)[center_index]
|
|
|
|
|
|
|
|
|
|
y = ret.getReadable(0)[_range[0] : _range[1]]
|
|
x = ret.getPositions(0)[_range[0]: _range[1]]
|
|
#x = enforce_monotonic(x)
|
|
#(normalization, mean_val, sigma) = fit_gaussian([-v for v in y], x)
|
|
|
|
|
|
closest_x = x[y.indexOf(min(y))]
|
|
closest_y = y[y.indexOf(min(y))]
|
|
|
|
|
|
|
|
if closest_x is None or closest_x <= ret.getPositions(0)[first_index] or closest_x >= ret.getPositions(0)[last_index]:
|
|
raise Exception("Invalid Fit")
|
|
|
|
|
|
|
|
center_offset = center_positon-closest_y
|
|
#center_offset = current_positon-closest_y
|
|
|
|
|
|
|
|
p=get_plots()[0]
|
|
p.addMarker(closest_y, p.AxisId.X, str(closest_y), Color.GREEN)
|
|
|
|
robot.set_motors_enabled(True)
|
|
robot_x.move(closest_x)
|