[PYTHON] Try using cheap LiDAR (Camsense X1)

Introduction

When I was wandering around AliExpress, LiDAR sold for a whopping $ 19 (including shipping). https://ja.aliexpress.com/item/4000632680397.html This time, I bought the cheap LiDAR. It seems that communication specifications and libraries are not officially released.

The identity of super-discount LiDAR

The manufacturer etc. are not written on the AliExpress page, There was a sticker "Camsense X1" in the image, so this seems to be the model of the sensor. If you search for Camsense X1, it looks like a product launched in 2017 by the brand Camsense. http://en.camsense.cn/index.php?c=article&id=116

specification

The Camsense page has the following simple specifications.

The measurement frequency is not the rotation frequency, but the number of steps that can be taken for distance data per second. Other specifications that I found after using it are as follows.

The angular resolution is not constant, and if you touch it with your hand to slow down the motor rotation, the angular resolution will become as fine as 0.5 degrees. The data acquisition cycle is not synchronized with the rotation, and it seems that the time is fixed.

Connect

There are three lines to connect to the outside: VCC, GND, and TX. I didn't have a connector, so I tried my best to connect it.

If you want to communicate with a PC, you need a USB-UART converter because UART communication is not possible directly. I used something like this. (I bought it a long time ago and forgot where I bought it)

The Raspberry Pi has a 3.3V UART so you should be able to use it as is.

This is how the Camsense X1 connects to your PC.

Caution

In the case of Windows, if data comes continuously from the serial port, it will be recognized as a serial mouse, and the mouse cursor will move here and there and operations that should not be done may be performed. Therefore, in the case of Windows, it is better to turn off the function that recognizes it as a serial mouse. A serial mouse is a very old mouse that was used before USB and PS / 2, and it doesn't mean that the current mouse can't be used. It's easy to do, with a registry editor HKEY_LOCAL_MACHINE\System\CurrentControlSet\Services\sermouse I think the value of the Start key of is 3, so I just set it to 4. https://stackoverflow.com/questions/9226082/device-misdetected-as-serial-mouse

Communication protocol

The person who analyzed the communication protocol explained it on github, so I used it as a reference. https://github.com/Vidicon/camsense-X1

It is a 36-byte packet containing the following distance and intensity data in 8 steps each. It is output endlessly.

The last 2 bytes seem to be CRC, but I tried to calculate with some generated polynomials and removed the header from the calculation range, but I could not find a match.

Data acquisition program (Python)

I wrote the code to get the data of Camsense X1 and draw it with Python. I also put it on github. https://github.com/j-fujimoto/CamsenseX1

Since the angular resolution changes depending on the rotation speed, the number of data for one lap is not determined, but the number of data for one lap of the angular resolution at the time of stability is secured. It requires pyserial and matplotlib to work.

Operation check environment

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):
        #First, read down to the header
        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)
        # "<" :Little endian, "H" :2-byte unsigned data, "B" :1-byte unsigned data
        (rotationSpeedTmp, startAngleTmp) = struct.unpack_from("<2H", tmp)
        self.rpm = rotationSpeedTmp / 64
        startAngle = (startAngleTmp - 0xa000) / 64
        #Prepare an array to store distance and intensity data
        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()

            #If it is near 0 degrees, the magnitude relationship between the start angle and the end angle may be reversed, so add 360 degrees to the end angle to maintain the magnitude relationship.
            if endAngle < startAngle:
                endAngle += 360

            #When the start angle becomes small, it is a place of 0 degrees, so set the data update flag
            if (startAngle - preStartAngle < 0):
                self.dataObtained = True
            preStartAngle = startAngle

            #Convert angles to radians
            startAngleRad = startAngle * math.pi / 180 * (-1 if self.isInvert else 1)
            endAngleRad = endAngle * math.pi / 180 * (-1 if self.isInvert else 1)
            #Calculate the angle per step
            angleIncrement = (endAngleRad - startAngleRad) / len(distanceTmp)
            #Exclusive control start
            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
            #Exclusive control end
            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()

    #Generate polar graph
    dist = plt.subplot(111, polar = True)
    #Set the initial value of the maximum distance to be displayed to 2000
    rmax = 2000

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

    while True:
        if capture.dataObtained:
            #Initialize drawing
            plt.cla()

            #Exclusive control start
            capture.lock.acquire()
            #Plot distance data(Draw as a scatter plot)
            dist.scatter(list(capture.theta), list(capture.distance), c = "blue", s = 5)
            #Plot intensity data(Draw by connecting with lines)
            dist.plot(list(capture.theta), list(capture.intensity), c = "orange", linewidth = 1)
            #Since the data has been drawn, lower the data acquisition flag.
            capture.dataObtained = False
            #Exclusive control start
            capture.lock.release()

            #Set the top of the screen to 0 degrees
            dist.set_theta_offset(math.pi / 2)
            #Set the maximum value of the distance value to be displayed
            dist.set_rmax(rmax)
            #Drawing process
            plt.pause(0.01)
            #Get the currently set maximum display distance value
            rmax = dist.get_rmax()

        else:
            time.sleep(0.01)

The state of drawing the data is like this. The blue dots draw the distance data, and the orange lines represent the intensity data.

Other public programs

Node for ROS1: https://github.com/Vidicon/camsense_driver Made by Vidicon (Bram Fenijn) Node for ROS2: https://github.com/rossihwang/ros2_camsense_x1 Made by rossihwang Program for STM32: https://github.com/anhui1995/Camsense_X1

in conclusion

It's amazing that LiDAR with this performance can be purchased for $ 19. With Ethernet-connected LiDAR, it is quite difficult to process with an inexpensive microcomputer, but Since Camsense X1 is a UART, it can be easily processed even with an inexpensive microcomputer. The rotation cycle is quite slow, and the distance that can be obtained is short, so I think it is difficult to use for robots that move quickly, but I think it's good enough for hobbies