我正试图读取距离数据从Camsense X1激光雷达与覆盆子皮零w。我能够获得距离数据,但问题开始于绘图方面。Matplotlib绘制初始数据,但不更新图形。我尝试了为这个传感器编写的其他脚本,但结果是一样的
脚本源:https://memotut.com/en/819aced4d48efd97c79f/
其他脚本:
https://github.com/Takahiro55555/camsenseX1-operation-test/blob/master/src/capture.pyhttps://github.com/Vidicon/camsense-X1
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 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)
目前没有回答
相关问题 更多 >
编程相关推荐