if __name__ == "__main__":
ydlidar.os_init()
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, 512000)
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF)
laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL)
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0)
laser.setlidaropt(ydlidar.LidarPropSampleRate, 20)
laser.setlidaropt(ydlidar.LidarPropSingleChannel, False)
ret = laser.initialize()
if ret:
ret = laser.turnOn()
scan = ydlidar.LaserScan()
while ret and ydlidar.os_isOk():
r = laser.doProcessSimple(scan)
if r:
print("Scan received[", scan.stamp, "]:", scan.points.size(),
"ranges is [", 1.0 / scan.config.scan_time, "]Hz")
else:
print("Failed to get Lidar Data.")
time.sleep(0.05)
laser.turnOff()
laser.disconnecting()
def testOSInitIsWrappedCorrectly(self):
print("test os init.......")
ydlidar.os_init()
self.assertTrue(ydlidar.os_isOk())
ydlidar.os_shutdown()
self.assertFalse(ydlidar.os_isOk())
laser.setlidaropt(ydlidar.LidarPropFixedResolution, True)
laser.setlidaropt(ydlidar.LidarPropInverted, True)
laser.setlidaropt(ydlidar.LidarPropSingleChannel, False)
laser.setlidaropt(ydlidar.LidarPropIntenstiy, True) # required for G2B, otherwise the laser will NOT turn on
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0)
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0)
ret = laser.initialize()
if ret:
ret = laser.turnOn()
scan = ydlidar.LaserScan()
time.sleep(5)
count = 0
while ret and ydlidar.os_isOk() and count <= 1000:
r = laser.doProcessSimple(scan)
if r:
print("Scan received[", scan.stamp, "]:",
scan.points.size(), "ranges is [",
1.0 / scan.config.scan_time, "]Hz")
else :
print("Failed to get Lidar Data")
sys.exit()
pts = []
for point in scan.points:
x = point.range * np.cos(point.angle)
y = point.range * np.sin(point.angle)
z = 0.0