def connect(rplidar, ins):
print "Connecting to RPLIDAR at "+rplidar.device+"..."
if rplidar.connect() == False:
print "RPLIDAR failed to connect!"
exit(1)
print "Connected to RPLIDAR!"
print "Connecting to INS at "+ins.device+"..."
if ins.connect() == False:
print "INS failed to connect!"
exit(1)
print "Connected to INS!"
count = 0
while(count < points):
buffer_heading[count] = ins.readline()
count = count + 1
print "INS collection time: "+str(time.time() - start_time)
return
try:
t1 = Thread(target=read_lidar)
t2 = Thread(target=read_ins)
t1.start()
t2.start()
t1.join()
t2.join()
for number in buffer_theta:
ft.write(str(number)+",")
for number in buffer_dist:
fd.write(str(number)+",")
for number in buffer_heading:
fh.write(str(number)+",")
return
except:
return
lidar = rplidar(device="/dev/ttyUSB8")
lidar.connect()
ins = ins()
ins.connect()
write_file()
ft.close()
fd.close()
fh.close()
exit(0)
