def read_scan_infos():
try:
path = constants.dataPath + r'/scan_infos.txt'
height = np.loadtxt(path)
return (height)
except:
cust_print(' Cannot read info file!')
def read_data_lidar(lidarNb):
try:
path = constants.dataPath + r'/DatasL' + str(lidarNb + 1) + '.txt'
data = np.loadtxt(path, dtype='d', delimiter=' ')
return (data)
except:
cust_print(' Cannot read data file!')
def SDM_algorithm(points):
P = init_SDM(points)
# Evaluate bspline
bspl = bspline(P)
#bspl.plot_curve(True)
#plt.show()
try:
bspl, error = iter_SDM(points, bspl)
except:
cust_print("An error occured while reconstructing the contour!")
bspl.plot_curve(False)
return bspl, error
def wait_servos_moving(self):
for lidarNb in range(constants.nb_of_lidars):
timeout = 100 # 10 seconds
while True:
try:
while self.serial_connection.is_moving(constants.servosIDs[lidarNb]) == True:
timeout -= 1
if timeout <= 0:
cust_print(" Problem with the servo" + str(lidarNb+1) + "!")
self.disconnect()
return -1
time.sleep(0.1)
break
except:
pass
return 0
def mark_zeros_line(bspl, esd, tresh):
zeros = np.full(bspl.n_c * 2, False)
for i in range(2 * bspl.n_c):
sum_line = 0
for j in range(2 * bspl.n_c):
sum_line += esd[i][j]
if np.abs(sum_line) < tresh:
zeros[i] = True
cpt = 0
for i in zeros:
if i == False:
cpt += 1
if cpt < 2:
cust_print("Cannot minimize SD error!")
raise
return zeros
def check_link_state(self):
for lidarNb in range(constants.nb_of_lidars):
try:
self.lidars[lidarNb].stop()
self.lidars[lidarNb].get_health()
except:
cust_print('Link error with lidar' + str(lidarNb+1))
self.disconnect()
return -1
try:
if self.serial_connection.ping(constants.servosIDs[lidarNb]) == False:
raise
except:
cust_print('Link error with servo' + str(lidarNb+1))
self.disconnect()
return -1
return 0
def contour():
#path = r'C:\Users\Toshiba\Documents\Vincent MAIRE\lidar_waist_scan\data\data_test.txt'
#datas = np.loadtxt(path, dtype='d', delimiter=' ')
#plt.figure(utility.figMerge)
#plt.gca().set_aspect('equal')
#plt.plot(datas[:,0], datas[:,1], '.k', ms=3)
#for i in range(len(datas)):
# points[i] = [datas[i,0],datas[i,1]]
datas = utility.mergedPointsXY
points = np.zeros((len(datas), 2))
for i in range(len(datas)):
points[i] = [datas[i].x, datas[i].y]
bspl, error = SDM_algorithm(points)
circum = compute_circumference(bspl)
cust_print('\nCircumference: ' + str(format(circum, '.2f')) + 'mm')
def single_scan(self,current_angle_z,meas_nb,erase_file):
file = []
iterMeas = []
datasLeft = []
done = []
for lidarNb in range(constants.nb_of_lidars):
try:
path = constants.dataPath + r'/DatasL' + str(lidarNb+1) + '.txt'
if erase_file == True:
file.append(open(path,'w'))
else:
file.append(open(path,'a'))
except:
cust_print(' Cannot open file for lidar' + str(lidarNb+1) + '!')
self.disconnect()
return -1
try:
iterMeas.append(self.lidars[lidarNb].iter_measures(max_buf_meas=10000))
except:
cust_print(' Cannot communicate with lidar' + str(lidarNb+1) + '!')
return -1
datasLeft.append(constants.nbOfDatasToRetrieve)
done.append(False)
try:
while False in done:
for lidarNb in range(constants.nb_of_lidars):
if done[lidarNb] == False:
datas = next(iterMeas[lidarNb])
angle = -1*((datas[2]+constants.offset_angle_lidar+180.0)%360 - 180.0)
dist = datas[3]
# First selection of points
if angle >= -30 and angle <= +30 and dist > 0:
file[lidarNb].write(str(angle) + ' ' + str(current_angle_z) + ' ' + str(dist) + '\n')
datasLeft[lidarNb] -= 1
if datasLeft[lidarNb] < 1:
done[lidarNb] = True
except:
cust_print(' Cannot retrieve datas on lidar ' + str(lidarNb+1) + '!')
self.disconnect()
return -1
for lidarNb in range(constants.nb_of_lidars):
try:
self.lidars[lidarNb].stop()
except:
cust_print(' Cannot communicate with lidar'+ str(lidarNb+1) + '!')
return -1
file[lidarNb].close()
return 0
def compute_algo_circumference():
# Time measurement
start = time.clock()
# Read lidars datas
lidarsSet.read_datas_files()
# Plot raw datas
lidarsSet.plot_raw_datas()
# Compute raw datas into merged datas
lidarsSet.compute_raw_datas()
# Remove outliers
if len(utility.mergedPointsXY) > constants.min_points:
#utility.remove_outliers()
# Compute contour
contour.contour()
cust_print("Circumference computed in: " + str(time.clock() - start) +
' sec.')
# Show the plots
if constants.disp_charts:
cust_print('\nClose all figures to continue...\n')
plt.show(block=True)
else:
cust_print(
' Not enough data point have been read in the expected area!')
def disconnect(self):
cust_print('Disconnecting to lidars and servos...')
self.serial_connection.close()
cust_print(' Disconnected to servos serial')
for lidarNb in range(constants.nb_of_lidars):
self.lidars[lidarNb].stop()
self.lidars[lidarNb].disconnect()
time.sleep(0.25) # To avoid to drain too much current
cust_print(' Disconnected to lidar' + str(lidarNb+1))
self.lidars[:] = []
self.areConnected = 0
cust_print('\nClose all figures to continue...\n')
plt.show(block=True)
else:
cust_print(
' Not enough data point have been read in the expected area!')
# Ignore warnings
warnings.simplefilter("ignore")
# Open bb serial
if constants.use_bb_serial:
bb_serial.bb_ser = bb_serial.bitbang_serial(constants.bb_serial_RX,
constants.bb_serial_TX,
constants.bb_baudrate)
# Init lidars infos
lidarsSet = lidars.setOfLidars()
# Init lidars
drv = lid.driverLidars()
end = False
while end == False:
try:
disp_menu()
end = execute_actions()
except:
cust_print("An error occured")
def execute_actions():
choice = cust_read(' Waiting for input: ')
if choice == 'q' or choice == 'Q':
if drv.areConnected == 1:
drv.disconnect()
cust_print("Program ended")
bb_serial.bb_ser.close_bb_serial()
return True
if drv.areConnected == 0:
if choice == '1':
# Connect to lidar
drv.connect()
elif choice == '2':
lidarsSet.read_datas_files()
lidarsSet.plot_raw_datas()
if constants.disp_charts:
cust_print('\nClose all figures to continue...\n')
plt.show(block=True)
elif choice == '3':
# Compute circumference
compute_algo_circumference()
else:
cust_print(' Wrong input!')
else:
if choice == '1':
# Disconnect to lidar
drv.disconnect()
elif choice == '2':
if drv.check_link_state() == 0:
drv.start_motors()
elif choice == '3':
if drv.check_link_state() == 0:
drv.stop_motors()
elif choice == '4':
run_scan = False
try:
height = int(
cust_read(' Please enter patient\'s height in cm: '))
if height > 0:
run_scan = True
else:
raise
except:
cust_print(' Incorrect height!')
# Data scanning
if run_scan:
if drv.check_link_state() == 0:
drv.scan_datas(height)
elif choice == '5':
lidarsSet.read_datas_files()
lidarsSet.plot_raw_datas()
if constants.disp_charts:
cust_print('\nClose all figures to continue...\n')
plt.show(block=True)
elif choice == '6':
compute_algo_circumference()
else:
cust_print(' Wrong input!')
return False
def connect(self):
cust_print('Connecting to lidars and servos...')
self.lidars = []
self.serial_connection = []
try:
# Connect to the serial port
self.serial_connection = Connection(port="/dev/ttyAMA0", baudrate=57600, rpi_gpio=True)
except:
cust_print(' Cannot open serial connection for servos!')
#return -1
for lidarNb in range(constants.nb_of_lidars):
try:
self.lidars.append(RPLidar(constants.serialPort[lidarNb],baudrate=115200))
self.lidars[lidarNb].connect()
except:
cust_print(' Cannot connect to the lidar' + str(lidarNb+1) + '!')
return -1
cust_print(' Connected to lidar' + str(lidarNb+1))
try:
# Try to ping the motor
if self.serial_connection.ping(constants.servosIDs[lidarNb]) == False:
raise
self.serial_connection.set_speed(BROADCAST_ID,constants.servosSpeed)
self.servos_goto(constants.servosIDs[lidarNb],0)
except:
cust_print(' Cannot connect to the servo' + str(lidarNb+1) + '!')
return -1
cust_print(' Connected to servo' + str(lidarNb+1))
time.sleep(0.25) # To avoid a too high current drain
self.areConnected = 1
return 0
def scan_datas(self,stature):
cust_print('Datas scanning...')
# Get height from stature
lin_coeffs_navel_height = [0.66410011, -105.89395578]
height = (stature*10)*lin_coeffs_navel_height[0] + lin_coeffs_navel_height[1]
cust_print(" Target: " + str(int(height/10)) + "cm")
self.start_motors()
# Clean the infos file
try:
infoFile = open(constants.dataPath + r'/scan_infos.txt','w')
except:
cust_print(" Cannot open infos file")
return -1
[inclination_array,nbMes_array] = compute_angle_array_for_scan(height)
for i in range(len(inclination_array)):
try:
# Broadcast moving action to all servos
self.servos_goto(BROADCAST_ID,inclination_array[i])
except:
cust_print(" Problem with the serial connection!")
self.disconnect()
return -1
if self.wait_servos_moving() != 0:
return -1
if i == 0:
if self.single_scan(inclination_array[i],nbMes_array[i],True) != 0:
return -1
else:
if self.single_scan(inclination_array[i],nbMes_array[i],False) != 0:
return -1
# Write the height of scan in the info file
try:
infoFile = open(constants.dataPath + r'/scan_infos.txt','a')
infoFile.write(str(height))
except:
cust_print(" Cannot open infos file")
return -1
try:
# Broadcast moving action to all servos
self.servos_goto(BROADCAST_ID,0)
except:
cust_print(" Problem with the serial connection!")
self.disconnect()
return -1
cust_print(' All the datas have been successfully retrieved.')
self.stop_motors()
return 0
def squared_dist(dist, rad, Ta_tk, No_tk, tk, neigh, bspl, points):
esd = np.zeros((2 * bspl.n_c, 2 * bspl.n_c))
const = np.zeros(2 * bspl.n_c)
n = bspl.n_c
bi_bj = np.zeros((n, n))
for k in range(len(points)):
# Basis elements
tmp_b_terms = bspl.get_basis(tk[k])
tmp_b_terms[:ORDER] += tmp_b_terms[-ORDER:]
b_terms = tmp_b_terms[:-ORDER]
# SDM
if dist[k] >= 0 and dist[k] < rad[k]:
neigh_pt_norm = (neigh[k][0] - points[k][0]) * No_tk[k, 0] + (
neigh[k][1] - points[k][1]) * No_tk[k, 1]
neigh_pt_norm_Nox = neigh_pt_norm * No_tk[k, 0]
neigh_pt_norm_Noy = neigh_pt_norm * No_tk[k, 1]
Nox_Nox = No_tk[k, 0]**2
Nox_Noy = No_tk[k, 0] * No_tk[k, 1]
Noy_Noy = No_tk[k, 1]**2
for i in range(n):
for j in range(n):
bi_bj = b_terms[i] * b_terms[j]
esd[i, j] += bi_bj * Nox_Nox
esd[i, j + n] += bi_bj * Nox_Noy
esd[i + n, j] += bi_bj * Nox_Noy
esd[i + n, j + n] += bi_bj * Noy_Noy
const[i] -= b_terms[i] * neigh_pt_norm_Nox
const[i + n] -= b_terms[i] * neigh_pt_norm_Noy
elif dist[k] < 0:
neigh_pt_norm = (neigh[k][0] - points[k][0]) * No_tk[k, 0] + (
neigh[k][1] - points[k][1]) * No_tk[k, 1]
neigh_pt_norm_Nox = neigh_pt_norm * No_tk[k, 0]
neigh_pt_norm_Noy = neigh_pt_norm * No_tk[k, 1]
prodNeighPtTang = (neigh[k][0] - points[k][0]) * Ta_tk[k, 0] + (
neigh[k][1] - points[k][1]) * Ta_tk[k, 1]
dist_distRad = dist[k] / (dist[k] - rad[k])
dist_distRad_Tax_Tax = dist_distRad * (Ta_tk[k, 0]**2)
dist_distRad_Tax_Tay = dist_distRad * Ta_tk[k, 0] * Ta_tk[k, 1]
dist_distRad_Tay_Tay = dist_distRad * (Ta_tk[k, 1]**2)
Nox_Nox = No_tk[k, 0]**2
Nox_Noy = No_tk[k, 0] * No_tk[k, 1]
Noy_Noy = No_tk[k, 1]**2
for i in range(n):
for j in range(n):
bi_bj = b_terms[i] * b_terms[j]
esd[i][j] += bi_bj * dist_distRad_Tax_Tax + bi_bj * Nox_Nox
esd[i, j +
n] += bi_bj * dist_distRad_Tax_Tay + bi_bj * Nox_Noy
esd[i + n,
j] += bi_bj * dist_distRad_Tax_Tay + bi_bj * Nox_Noy
esd[i + n, j +
n] += bi_bj * dist_distRad_Tay_Tay + bi_bj * Noy_Noy
const[i] -= dist_distRad * b_terms[i] * Ta_tk[
k, 0] * prodNeighPtTang + b_terms[i] * neigh_pt_norm_Nox
const[i + n] -= dist_distRad * b_terms[i] * Ta_tk[
k, 1] * prodNeighPtTang + b_terms[i] * neigh_pt_norm_Noy
else:
cust_print("Error rad < dist: " + str(rad) + '<', dist)
raise
return esd, const
def disp_menu():
cust_print("")
cust_print("")
cust_print('----MENU----')
if drv.areConnected == 0:
cust_print('1: Connect to lidars')
cust_print('2: Plot datas')
cust_print('3: Compute circumference')
else:
cust_print('1: Disconnect to lidars')
cust_print('2: Start motors')
cust_print('3: Stop motors')
cust_print('4: Launch scan')
cust_print('5: Plot datas')
cust_print('6: Compute circumference')
cust_print('q: Quit')
def iter_SDM(points, bspl):
iter_max = ITER_MAX
nb_iter = 0
temp_approx_error = math.inf
while True:
# Stop condition
if nb_iter >= iter_max:
break
# Compute point attributes
dist, rad, Ta_tk, No_tk, tk, neigh = compute_points_attributes(
points, bspl)
non_out_ind = get_non_outliers(dist)
# Remove the outliers
points = points[non_out_ind]
dist = dist[non_out_ind]
rad = rad[non_out_ind]
Ta_tk = Ta_tk[non_out_ind]
No_tk = No_tk[non_out_ind]
tk = tk[non_out_ind]
neigh = neigh[non_out_ind]
# Approximation error
approx_error = compute_approx_error(dist)
if nb_iter == 0:
cust_print(" Fit average error init: " + str(approx_error))
else:
cust_print(" Fit average error: " + str(approx_error))
# Stop conditions
if approx_error <= APPROXIMATION_ERROR_THRESHOLD:
break
if approx_error >= temp_approx_error:
bspl = copy.copy(temp_bspl)
break
# Temp variables
temp_approx_error = approx_error
temp_bspl = copy.copy(bspl)
# Objective function minimization
esd, esd_const = squared_dist(dist, rad, Ta_tk, No_tk, tk, neigh, bspl,
points)
reg, reg_const = compute_regularization(bspl)
# Regularization
fsd = 0.5 * esd + REGUL_WEIGHT * reg
const = 0.5 * esd_const + REGUL_WEIGHT * reg_const
# Zeros constraints for robustness
zeros = mark_zeros_line(bspl, fsd, ZERO_LINE_THRESHOLD)
fsd, const = apply_zeros_constraints(bspl, fsd, const, zeros)
# System solving
D = solve(fsd, const)
# Update spline
#print(np.mean(np.abs(D)))
P = move_ctrl_points(bspl, bspl.c, 1 * D, zeros)
bspl.update()
#bspl.plot_curve(True)
#plt.show()
nb_iter += 1
return bspl, approx_error