[PYTHON] Essayez d'utiliser LiDAR bon marché (Camsense X1)

introduction

Lorsque je me promenais sur AliExpress, LiDAR s'est vendu pour 19 $ (frais de port compris). https://ja.aliexpress.com/item/4000632680397.html Cette fois, j'ai acheté le LiDAR bon marché. Il semble que les spécifications de communication et les bibliothèques ne soient pas officiellement publiées.

L'identité du LiDAR super-discount

Le fabricant etc. n'est pas écrit sur la page AliExpress, Il y avait un autocollant "Camsense X1" dans l'image, donc cela semble être le modèle du capteur. Si vous recherchez Camsense X1, cela ressemble à un produit lancé en 2017 par la marque Camsense. http://en.camsense.cn/index.php?c=article&id=116

spécification

La page Camsense présente les spécifications simples suivantes.

La fréquence de mesure n'est pas la fréquence de rotation, mais le nombre de pas que les données de distance peuvent être prises par seconde. Les autres spécifications que j'ai trouvées après son utilisation sont les suivantes.

La résolution angulaire n'est pas constante et si vous la touchez avec votre main pour ralentir la rotation du moteur, la résolution angulaire deviendra aussi fine que 0,5 degré. Le cycle d'acquisition des données n'est pas synchronisé avec la rotation, et il semble que l'heure soit fixe.

Lien

Il y a trois lignes pour se connecter à l'extérieur: VCC, GND et TX. Je n'avais pas de connecteur, alors j'ai fait de mon mieux pour le connecter.

Si vous souhaitez communiquer avec un PC, vous avez besoin d'un convertisseur USB-UART car la communication UART n'est pas possible directement. J'ai utilisé quelque chose comme ça. (Je l'ai acheté il y a longtemps et j'ai oublié où je l'ai acheté)

Raspberry Pi a un UART 3.3V, vous devriez donc pouvoir l'utiliser tel quel.

C'est ainsi que le Camsense X1 se connecte à votre PC.

Mise en garde

Dans le cas de Windows, si les données proviennent continuellement du port série, elles seront reconnues comme souris série, et le curseur de la souris ira ici et là et les opérations qui ne devraient pas être effectuées pourront être effectuées. Par conséquent, dans le cas de Windows, il est préférable de désactiver la fonction qui le reconnaît comme une souris série. Une souris série est une souris très ancienne qui était utilisée avant USB et PS / 2, et cela ne signifie pas que la souris actuelle ne peut pas être utilisée. C'est facile à faire, avec un éditeur de registre HKEY_LOCAL_MACHINE\System\CurrentControlSet\Services\sermouse Je pense que la valeur de la touche Start de est 3, donc je la mets simplement à 4. https://stackoverflow.com/questions/9226082/device-misdetected-as-serial-mouse

Protocole de communication

La personne qui a analysé le protocole de communication l'a expliqué sur github, je l'ai donc utilisé comme référence. https://github.com/Vidicon/camsense-X1

Il s'agit d'un paquet de 36 octets contenant les données de distance et de force suivantes en 8 étapes chacune. Il est produit à l'infini.

Les 2 derniers octets semblent être CRC, mais j'ai essayé de calculer avec des polynômes générés et j'ai supprimé l'en-tête de la plage de calcul, mais je n'ai pas trouvé de correspondance.

Programme d'acquisition de données (Python)

J'ai écrit le code pour obtenir les données de Camsense X1 et les dessiner avec Python. Je l'ai également mis sur github. https://github.com/j-fujimoto/CamsenseX1

Puisque la résolution angulaire change en fonction de la vitesse de rotation, le nombre de données pour un tour n'est pas déterminé, mais le nombre de données pour un tour de la résolution angulaire au moment de la stabilité est garanti. Il nécessite pyserial et matplotlib pour fonctionner.

Environnement de vérification de fonctionnement

camsenseX1.py

import matplotlib.pyplot as plt
import math
import serial
import sys
import struct
import time
import threading

class Capture:
    def __init__(self, serialPort, dataSize = 460, isInvert = True):
        self.theta = [0] * dataSize
        self.distance = [0] * dataSize
        self.intensity = [0] * dataSize
        self.writePos = 0
        self.serial = serial.Serial(port = serialPort, baudrate = 115200)
        self.dataSize = dataSize
        self.thread = threading.Thread(target = self.getData)
        self.lock = threading.Lock()
        self.isInvert = isInvert
        self.dataObtained = False
        self.rpm = 0

    def getDataUnit(self):
        #Tout d'abord, lisez l'en-tête
        header = b"\x55\xAA\x03\x08"
        headerPos = 0
        while True:
            tmp = self.serial.read(1)
            if tmp[0] == header[headerPos]:
                headerPos += 1
                if headerPos == len(header):
                    break
            else:
                headerPos = 0

        tmp = self.serial.read(4)
        # "<" :Petit indien, "H" :Données non signées sur 2 octets, "B" :Données non signées 1 octet
        (rotationSpeedTmp, startAngleTmp) = struct.unpack_from("<2H", tmp)
        self.rpm = rotationSpeedTmp / 64
        startAngle = (startAngleTmp - 0xa000) / 64
        #Préparez un tableau pour stocker les données de distance et de force
        distanceTmp = [0] * 8
        intensityTmp = [0] * 8
        for i in range(8):
            tmp = self.serial.read(3)
            (distanceTmp[i], intensityTmp[i]) = struct.unpack_from("<HB", tmp)
        tmp = self.serial.read(4)
        (endAngleTmp, crc) = struct.unpack_from("<2H", tmp)
        endAngle = (endAngleTmp - 0xa000) / 64

        return (distanceTmp, intensityTmp, startAngle, endAngle)

    def getData(self):
        preStartAngle = 0
        while True:
            (distanceTmp, intensityTmp, startAngle, endAngle) = self.getDataUnit()

            #Dans le cas d'environ 0 degré, la relation de grandeur entre l'angle de départ et l'angle de fin peut être inversée, donc ajoutez 360 degrés à l'angle de fin pour maintenir la relation de grandeur.
            if endAngle < startAngle:
                endAngle += 360

            #Lorsque l'angle de départ devient petit, c'est un endroit de 0 degré, définissez donc l'indicateur de mise à jour des données
            if (startAngle - preStartAngle < 0):
                self.dataObtained = True
            preStartAngle = startAngle

            #Convertir l'angle en radian
            startAngleRad = startAngle * math.pi / 180 * (-1 if self.isInvert else 1)
            endAngleRad = endAngle * math.pi / 180 * (-1 if self.isInvert else 1)
            #Calculez l'angle par pas
            angleIncrement = (endAngleRad - startAngleRad) / len(distanceTmp)
            #Démarrage de contrôle exclusif
            self.lock.acquire()
            for i in range(len(distanceTmp)):
                self.theta[self.writePos] = startAngleRad + angleIncrement * i
                self.distance[self.writePos] = distanceTmp[i]
                self.intensity[self.writePos] = intensityTmp[i]
                self.writePos += 1
                if self.writePos >= self.dataSize:
                    self.writePos = 0
            #Fin de contrôle exclusif
            self.lock.release()

    def run(self):
        self.thread.start()

    def stop(self):
        self.thread.stop()

if __name__ == "__main__":
    if len(sys.argv) == 1:
        print("You must specify serial port! ex) " + sys.argv[0] + " COM2")
        quit()

    #Générer un graphique de coordonnées polaires
    dist = plt.subplot(111, polar = True)
    #Réglez la valeur initiale de la distance maximale à afficher sur 2000
    rmax = 2000

    capture = Capture(sys.argv[1], dataSize = 480, isInvert = True)
    capture.run()
    preTime = time.time()

    while True:
        if capture.dataObtained:
            #Initialiser le dessin
            plt.cla()

            #Démarrage de contrôle exclusif
            capture.lock.acquire()
            #Tracer les données de distance(Dessiner sous forme de diagramme de dispersion)
            dist.scatter(list(capture.theta), list(capture.distance), c = "blue", s = 5)
            #Tracer les données d'intensité(Dessiner en se connectant avec des lignes)
            dist.plot(list(capture.theta), list(capture.intensity), c = "orange", linewidth = 1)
            #Puisque les données ont été dessinées, abaissez le drapeau d'acquisition de données.
            capture.dataObtained = False
            #Démarrage de contrôle exclusif
            capture.lock.release()

            #Réglez le haut de l'écran sur 0 degré
            dist.set_theta_offset(math.pi / 2)
            #Définir la valeur maximale de la valeur de distance à afficher
            dist.set_rmax(rmax)
            #Processus de dessin
            plt.pause(0.01)
            #Obtenir la valeur de distance d'affichage maximale actuellement définie
            rmax = dist.get_rmax()

        else:
            time.sleep(0.01)

L'état de dessin des données est comme ceci. Les données de distance sont dessinées avec des points bleus et les données d'intensité sont affichées avec des lignes orange.

Autres programmes publics

Node pour ROS1: https://github.com/Vidicon/camsense_driver Fabriqué par Vidicon (Bram Fenijn) Node pour ROS2: https://github.com/rossihwang/ros2_camsense_x1 Fabriqué par rossihwang Programme pour STM32: https://github.com/anhui1995/Camsense_X1

en conclusion

C'est incroyable que LiDAR avec cette performance puisse être acheté pour 19 $. Avec LiDAR connecté via Ethernet, il est assez difficile de traiter avec un micro-ordinateur peu coûteux, mais Puisque Camsense X1 est un UART, il peut être facilement traité même avec un micro-ordinateur bon marché. Le cycle de rotation est assez lent, et la distance qui peut être obtenue est courte, donc je pense que c'est difficile à utiliser pour des robots qui se déplacent rapidement, mais Je pense que c'est assez bon pour un usage amateur