Site icon AranaCorp

Utilizar um sensor Lidar com Python

Xukyo

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:

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

Fontes

Exit mobile version