Uso de un sensor Lidar con Python

Xukyo

hace 3 meses

En este tutorial, veremos cómo configurar un mapa utilizando un sensor Lidar en Python. El sensor Lidar se utiliza para encontrar su orientación en el espacio y para mapear.

Descripción del sensor Lidar

El sensor lidar es un sensor láser de distancia acoplado a un motor que lo hace girar. Se comporta como un radar, detectando obstáculos en 360 grados y cartografiando el espacio. Suele utilizarse en robótica para cartografiar y orientarse en un entorno y permitir el movimiento autónomo.

El sensor Lidar utiliza una tarjeta de interfaz serie para conectarlo a un ordenador a través de un puerto USB.

Para saber a qué puerto está conectado el dispositivo, vaya al administrador de dispositivos en la sección Puertos (COM y LPT) (aquí com3).

Pruebas Lidar con la herramienta oficial

Puede descargar la interfaz de la herramienta desde el sitio web oficial. Esta herramienta permite visualizar los mapas obtenidos mediante Lidar.

Una vez iniciado el software, basta con seleccionar el puerto del dispositivo y el modelo Lidar. Una vez conectado el Lidar, puede iniciar la adquisición.

Instalación del paquete Python

Para utilizar el sensor Lidar con Python, utilizamos la biblioteca PyLidar3.

python -m pip install Pylidar3

Script de prueba Lidar

Para comprobar que Pylidar ha sido instalado, puedes utilizar el siguiente script, que simplemente muestra la información del 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")

Visualización del resultado de la exploración

Para nuestro primer mapa, vamos a inicializar el objeto PyLidar3 y conectarnos al dispositivo

Obj = PyLidar3.YdLidarX4(port)
ret = Obj.Connect()

A continuación, vamos a definir algunos parámetros de escaneo:

duración del escaneo scanDuration
umbral de datos a registrar dataThr
winLim límite de ventana

Por último, vamos a lanzar la medición lidar y mostrar la adquisición con 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) &lt; scanDuration:
		data = next(gen)
		for angle in range(0,360):
			if(data[angle]&gt;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

Una vez ejecutado el script, podemos ver el proceso de mapeo en acción y el mapa actualizándose en tiempo real.

Gestión Lidar con roscado

Es posible gestionar la medición en un proceso diferente de la visualización para mejorar la fluidez. Como ejemplo, utilizaremos la librería threading. Crearemos una función de adquisición de datos scan(), que colocaremos en un hilo.

threading.Thread(target=scan).start()

Esto significa que podemos procesar los datos cartográficos sin interrumpir la adquisición de la medición.

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&gt;=buffnum-1:
			nb=0
		else:
			nb+=1

		for angle in range(0,360):
			if(data[angle]&gt;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) &lt; 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

Aplicación

Cartografía de un espacio
Posicionar un robot en el espacio y evitar obstáculos