script/calibration/ScanX.py

@@ -0,0 +1,81 @@
+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)