Created
May 26, 2023 17:56
-
-
Save PloxKevin/5fe40212cfcf41f974fc8de59ea95689 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import os | |
| import ydlidar | |
| import time | |
| import sys | |
| from matplotlib.patches import Arc | |
| import matplotlib.pyplot as plt | |
| import matplotlib.animation as animation | |
| import numpy as np | |
| RMAX = 32.0 | |
| fig = plt.figure() | |
| fig.suptitle('YDLidar LIDAR Monitor') | |
| lidar_polar = plt.subplot(polar=True) | |
| lidar_polar.autoscale_view(True,True,True) | |
| lidar_polar.set_rmax(RMAX) | |
| lidar_polar.grid(True) | |
| ports = ydlidar.lidarPortList(); | |
| port = "/dev/ydlidar"; | |
| for key, value in ports.items(): | |
| port = value; | |
| laser = ydlidar.CYdLidar(); | |
| laser.setlidaropt(ydlidar.LidarPropSerialPort, port); | |
| laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 230400) | |
| laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE); | |
| laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL); | |
| laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0); | |
| laser.setlidaropt(ydlidar.LidarPropSampleRate, 9); | |
| laser.setlidaropt(ydlidar.LidarPropSingleChannel, False); | |
| scan = ydlidar.LaserScan() | |
| def animate(num): | |
| r = laser.doProcessSimple(scan); | |
| if r: | |
| angle = [] | |
| ran = [] | |
| intensity = [] | |
| for point in scan.points: | |
| angle.append(point.angle); | |
| ran.append(point.range); | |
| intensity.append(point.intensity); | |
| lidar_polar.clear() | |
| lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', alpha=0.95) | |
| ret = laser.initialize(); | |
| if ret: | |
| ret = laser.turnOn(); | |
| if ret: | |
| ani = animation.FuncAnimation(fig, animate, interval=50) | |
| plt.savefig('lidar_monitor.png') # Save the figure as PNG | |
| plt.show() | |
| laser.turnOff(); | |
| laser.disconnecting(); | |
| plt.close(); |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment