Neste tutorial, vamos ver como configurar um mapa utilizando um sensor Lidar em Python. O sensor Lidar é utilizado para encontrar as suas coordenadas no espaço e para mapear.
Descrição do sensor Lidar
O sensor lidar é um sensor de distância a laser acoplado a um motor que o faz rodar. Comporta-se como um radar, detectando obstáculos a 360 graus e mapeando o espaço. É frequentemente utilizado em robótica para mapear e encontrar o seu caminho num ambiente e permitir o movimento autónomo.
O sensor Lidar utiliza uma placa de interface série para o ligar a um computador através de uma porta USB.
Para saber a que porta o dispositivo está ligado, vá ao Gestor de Dispositivos na secção Portas (COM e LPT) (aqui com3).
Testar o Lidar com a ferramenta oficial
A interface da ferramenta pode ser descarregada a partir do sítio Web oficial. Esta ferramenta permite-lhe visualizar os mapas obtidos com Lidar.
Uma vez iniciado o software, basta selecionar a porta do dispositivo e o modelo do Lidar. Quando o Lidar estiver ligado, pode iniciar a aquisição.
Instalar o pacote Python
Para utilizar o sensor Lidar com Python, utilizamos a biblioteca PyLidar3.
python -m pip install Pylidar3
Script de teste Lidar
Para verificar se o Pylidar foi instalado, pode utilizar o seguinte script, que mostra simplesmente as informações do dispositivo.
import PyLidar3
port = "COM3" #input("Enter port name which lidar is connected:") #windows
Obj = PyLidar3.YdLidarX4(port)
print("connecting to {}".format(port))
ret = Obj.Connect()
if(1):
print(ret)
print("device info: ",Obj.GetDeviceInfo())
Obj.Disconnect()
else:
print("Error connecting to device")
Apresentação do resultado da digitalização
Para o nosso primeiro mapa, vamos inicializar o objeto PyLidar3 e ligar-nos ao dispositivo
Obj = PyLidar3.YdLidarX4(port) ret = Obj.Connect()
De seguida, vamos definir alguns parâmetros de digitalização:
- duração da digitalização scanDuration
- limiar de dados a registar dataThr
- winLim limite da janela
Finalmente, vamos executar a medição lidar e apresentar a aquisição com matplotlib
import PyLidar3
import matplotlib.pyplot as plt
import math
import time
#init lidar
port = "COM3" #input("Enter port name which lidar is connected:") #windows
Obj = PyLidar3.YdLidarX4(port) #PyLidar3.your_version_of_lidar(port,chunk_size)
ret = Obj.Connect()
print(ret)
#ret = Obj.Connect()
#print(ret)
#parameters
scanDuration = 30 #scan for 30 seconds
dataThr = 1000 # disgard data below this value
winLim = 5000 # window limit in X and Y
#init data list on 360deg
x=[]
y=[]
for _ in range(360):
x.append(0)
y.append(0)
if(1):
deviceInfo = Obj.GetDeviceInfo()
print("device info : ",deviceInfo)
gen = Obj.StartScanning()
t = time.time() # start time
while (time.time() - t) < scanDuration:
data = next(gen)
for angle in range(0,360):
if(data[angle]>dataThr):
#x[angle] = data[angle] * math.cos(math.radians(angle))
#y[angle] = data[angle] * math.sin(math.radians(angle))
x[angle] = (x[angle] + data[angle] * math.cos(math.radians(angle))) / 2
y[angle] = (y[angle] + data[angle] * math.sin(math.radians(angle))) / 2
plt.clf()
plt.axis([-winLim,winLim,-winLim,winLim])
plt.title("Model: X4 Firmware: {} Hardware: {} SN: {}".format(deviceInfo['firmware_version'], deviceInfo['hardware_version'],deviceInfo['serial_number']))
plt.scatter(y, x, c='r', s=8)
plt.draw()
plt.pause(0.1)
Obj.StopScanning()
Obj.Disconnect()
else:
print("Error connecting to device")
plt.show()
Resultados
Depois de o script ter sido executado, podemos ver o processo de mapeamento em ação e o mapa a ser atualizado em tempo real.
Gestão de Lidar com threading
É possível gerir a medição num processo diferente da visualização para melhorar a fluidez. Como exemplo, utilizamos a biblioteca de threading. Vamos criar uma função de aquisição de dados scan(), que colocaremos numa thread.
threading.Thread(target=scan).start()
Isto significa que podemos processar os dados cartográficos sem interromper a aquisição da medição.
import PyLidar3
import matplotlib.pyplot as plt
import math
import time
import numpy as np
import threading
#init lidar
port = "COM3" #input("Enter port name which lidar is connected:") #windows
Obj = PyLidar3.YdLidarX4(port) #PyLidar3.your_version_of_lidar(port,chunk_size)
ret = Obj.Connect()
print(ret)
#ret = Obj.Connect()
#print(ret)
#parameters
scanDuration = 30 #scan for 30 seconds
dataThr = 1000 # disgard data below this value
winLim = 5000 # window limit in X and Y
buffnum = 1 # matrix size to average position
#init data list on 360deg
x = np.zeros((buffnum, 360))
y = np.zeros((buffnum, 360))
def scan():
nb=0
while is_scanning:
data = next(gen)
if nb>=buffnum-1:
nb=0
else:
nb+=1
for angle in range(0,360):
if(data[angle]>dataThr):
x[nb][angle] = data[angle] * math.cos(math.radians(angle))
y[nb][angle] = data[angle] * math.sin(math.radians(angle))
if(1):
deviceInfo = Obj.GetDeviceInfo()
print("device info : ",deviceInfo)
gen = Obj.StartScanning()
t = time.time() # start time
is_scanning = True
threading.Thread(target=scan).start()
while (time.time() - t) < scanDuration:
xmean = np.mean(x, axis=0)
ymean = np.mean(y, axis=0)
plt.clf()
plt.axis([-winLim,winLim,-winLim,winLim])
plt.title("Model: X4 Firmware: {} Hardware: {} SN: {}".format(deviceInfo['firmware_version'], deviceInfo['hardware_version'],deviceInfo['serial_number']))
#plt.scatter(y,x,c='r',s=8)
plt.scatter(ymean,xmean,c='r',s=8)
plt.draw()
plt.pause(0.05)
is_scanning = False
Obj.StopScanning()
Obj.Disconnect()
else:
print("Error connecting to device")
plt.show() #keep figure open at the end
Aplicação
- Mapeamento de um espaço
- Posicionar um robô no espaço e evitar obstáculos
