├── .gitignore ├── driver ├── Windows │ └── cp2102_driver.zip └── MacOS │ └── SiLabsUSBDriverDisk.dmg ├── examples ├── simple_scan.py ├── simple_express_scan.py └── check_connection.py ├── setup.py ├── pyrplidar_serial.py ├── LICENSE ├── README.md ├── pyrplidar.py ├── pyrplidar_protocol.py └── test └── test_pyrplidar_protocol.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | env/ 3 | dist/ 4 | sdist/ 5 | build/ 6 | *.egg-info/ 7 | -------------------------------------------------------------------------------- /driver/Windows/cp2102_driver.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hyun-je/pyrplidar/HEAD/driver/Windows/cp2102_driver.zip -------------------------------------------------------------------------------- /driver/MacOS/SiLabsUSBDriverDisk.dmg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hyun-je/pyrplidar/HEAD/driver/MacOS/SiLabsUSBDriverDisk.dmg -------------------------------------------------------------------------------- /examples/simple_scan.py: -------------------------------------------------------------------------------- 1 | from pyrplidar import PyRPlidar 2 | import time 3 | 4 | def simple_scan(): 5 | 6 | lidar = PyRPlidar() 7 | lidar.connect(port="/dev/ttyUSB0", baudrate=256000, timeout=3) 8 | # Linux : "/dev/ttyUSB0" 9 | # MacOS : "/dev/cu.SLAB_USBtoUART" 10 | # Windows : "COM5" 11 | 12 | 13 | lidar.set_motor_pwm(500) 14 | time.sleep(2) 15 | 16 | scan_generator = lidar.force_scan() 17 | 18 | for count, scan in enumerate(scan_generator()): 19 | print(count, scan) 20 | if count == 20: break 21 | 22 | lidar.stop() 23 | lidar.set_motor_pwm(0) 24 | 25 | 26 | lidar.disconnect() 27 | 28 | 29 | if __name__ == "__main__": 30 | simple_scan() 31 | -------------------------------------------------------------------------------- /examples/simple_express_scan.py: -------------------------------------------------------------------------------- 1 | from pyrplidar import PyRPlidar 2 | import time 3 | 4 | def simple_express_scan(): 5 | 6 | lidar = PyRPlidar() 7 | lidar.connect(port="/dev/ttyUSB0", baudrate=256000, timeout=3) 8 | # Linux : "/dev/ttyUSB0" 9 | # MacOS : "/dev/cu.SLAB_USBtoUART" 10 | # Windows : "COM5" 11 | 12 | 13 | lidar.set_motor_pwm(500) 14 | time.sleep(2) 15 | 16 | scan_generator = lidar.start_scan_express(4) 17 | 18 | for count, scan in enumerate(scan_generator()): 19 | print(count, scan) 20 | if count == 20: break 21 | 22 | lidar.stop() 23 | lidar.set_motor_pwm(0) 24 | 25 | 26 | lidar.disconnect() 27 | 28 | 29 | if __name__ == "__main__": 30 | simple_express_scan() 31 | -------------------------------------------------------------------------------- /examples/check_connection.py: -------------------------------------------------------------------------------- 1 | from pyrplidar import PyRPlidar 2 | 3 | 4 | def check_connection(): 5 | 6 | lidar = PyRPlidar() 7 | lidar.connect(port="/dev/ttyUSB0", baudrate=256000, timeout=3) 8 | # Linux : "/dev/ttyUSB0" 9 | # MacOS : "/dev/cu.SLAB_USBtoUART" 10 | # Windows : "COM5" 11 | 12 | 13 | info = lidar.get_info() 14 | print("info :", info) 15 | 16 | health = lidar.get_health() 17 | print("health :", health) 18 | 19 | samplerate = lidar.get_samplerate() 20 | print("samplerate :", samplerate) 21 | 22 | 23 | scan_modes = lidar.get_scan_modes() 24 | print("scan modes :") 25 | for scan_mode in scan_modes: 26 | print(scan_mode) 27 | 28 | lidar.disconnect() 29 | 30 | 31 | 32 | if __name__ == "__main__": 33 | check_connection() 34 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | setuptools.setup( 7 | name = "pyrplidar", 8 | version = "0.1.2", 9 | author = "Hyun-je", 10 | author_email = "bigae2@gmail.com", 11 | license = "MIT", 12 | description = "Full-featured python library for Slamtec RPLIDAR series", 13 | long_description = long_description, 14 | long_description_content_type = "text/markdown", 15 | url = "https://github.com/Hyun-je/pyrplidar", 16 | install_requires = ["pyserial"], 17 | packages = setuptools.find_packages(), 18 | py_modules = [ 19 | "pyrplidar", 20 | "pyrplidar_serial", 21 | "pyrplidar_protocol" 22 | ], 23 | platforms = "Cross Platform", 24 | classifiers = [ 25 | "Programming Language :: Python :: 3", 26 | "License :: OSI Approved :: MIT License", 27 | "Operating System :: OS Independent", 28 | ], 29 | ) 30 | -------------------------------------------------------------------------------- /pyrplidar_serial.py: -------------------------------------------------------------------------------- 1 | import serial 2 | 3 | 4 | class PyRPlidarSerial: 5 | 6 | def __init__(self): 7 | self._serial = None 8 | 9 | def open(self, port, baudrate, timeout): 10 | if self._serial is not None: 11 | self.disconnect() 12 | try: 13 | self._serial = serial.Serial(port, baudrate, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=timeout, dsrdtr=True) 14 | except serial.SerialException as err: 15 | print("Failed to connect to the rplidar") 16 | 17 | def close(self): 18 | if self._serial is None: 19 | return 20 | self._serial.close() 21 | 22 | def wait_data(self): 23 | pass 24 | 25 | def send_data(self, data): 26 | self._serial.write(data) 27 | 28 | def receive_data(self, size): 29 | return self._serial.read(size) 30 | 31 | def set_dtr(self, value): 32 | self._serial.dtr = value 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2019 The Python Packaging Authority 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PyRPlidar ![PyPI - Python Version](https://img.shields.io/pypi/pyversions/pyrplidar) ![PyPI](https://img.shields.io/pypi/v/pyrplidar) [![MIT License](https://img.shields.io/github/license/mashape/apistatus.svg)](https://github.com/Hyun-je/pyrplidar/blob/master/LICENSE) 2 | 3 | ![61845532-a5413e80-aede-11e9-9eee-db8438055619](https://user-images.githubusercontent.com/7419790/61871806-b14bf100-af1c-11e9-94a6-812b4f10930a.png) 4 | 5 | ## Introduction 6 | [PyRPlidar](https://github.com/Hyun-je/pyrplidar) is a python library for Slamtec RPLIDAR series. 7 | 8 | * Supports all series (A1, A2 and A3) 9 | * Implement all features of the device 10 | * Simple code & Easy to use 11 | * Use generator pattern (for performance) 12 | 13 | ![ezgif-5-3c5261a0b7fa](https://user-images.githubusercontent.com/7419790/66256236-b94ec980-e7c6-11e9-921e-c5098fce58b1.gif) 14 | 15 | 16 | ## Installation 17 | ```sh 18 | $ pip install pyrplidar 19 | ``` 20 | 21 | ## Example Code 22 | ```Python 23 | from pyrplidar import PyRPlidar 24 | 25 | lidar = PyRPlidar() 26 | lidar.connect(port="/dev/ttyUSB0", baudrate=256000, timeout=3) 27 | # Linux : "/dev/ttyUSB0" 28 | # MacOS : "/dev/cu.SLAB_USBtoUART" 29 | # Windows : "COM5" 30 | 31 | 32 | info = lidar.get_info() 33 | print("info :", info) 34 | 35 | health = lidar.get_health() 36 | print("health :", health) 37 | 38 | samplerate = lidar.get_samplerate() 39 | print("samplerate :", samplerate) 40 | 41 | 42 | scan_modes = lidar.get_scan_modes() 43 | print("scan modes :") 44 | for scan_mode in scan_modes: 45 | print(scan_mode) 46 | 47 | 48 | lidar.disconnect() 49 | ``` 50 | 51 | ## Documentation 52 | This library implement full specifications on the [protocol documentation](http://bucket.download.slamtec.com/ccb3c2fc1e66bb00bd4370e208b670217c8b55fa/LR001_SLAMTEC_rplidar_protocol_v2.1_en.pdf) of Slamtec. 53 | -------------------------------------------------------------------------------- /pyrplidar.py: -------------------------------------------------------------------------------- 1 | from pyrplidar_serial import PyRPlidarSerial 2 | from pyrplidar_protocol import * 3 | import pyrplidar_protocol 4 | 5 | 6 | 7 | class PyRPlidar: 8 | 9 | def __init__(self): 10 | self.lidar_serial = None 11 | self.measurements = None 12 | 13 | def __del__(self): 14 | self.disconnect() 15 | 16 | 17 | 18 | def connect(self, port="/dev/ttyUSB0", baudrate=115200, timeout=3): 19 | self.lidar_serial = PyRPlidarSerial() 20 | self.lidar_serial.open(port, baudrate, timeout) 21 | print("PyRPlidar Info : device is connected") 22 | 23 | 24 | def disconnect(self): 25 | if self.lidar_serial is not None: 26 | self.lidar_serial.close() 27 | self.lidar_serial = None 28 | print("PyRPlidar Info : device is disconnected") 29 | 30 | 31 | 32 | def send_command(self, cmd, payload=None): 33 | if self.lidar_serial == None: 34 | raise PyRPlidarConnectionError("PyRPlidar Error : device is not connected") 35 | 36 | self.lidar_serial.send_data(PyRPlidarCommand(cmd, payload).raw_bytes) 37 | 38 | def receive_discriptor(self): 39 | if self.lidar_serial == None: 40 | raise PyRPlidarConnectionError("PyRPlidar Error : device is not connected") 41 | 42 | discriptor = PyRPlidarResponse(self.lidar_serial.receive_data(RPLIDAR_DESCRIPTOR_LEN)) 43 | 44 | if discriptor.sync_byte1 != RPLIDAR_SYNC_BYTE1[0] or discriptor.sync_byte2 != RPLIDAR_SYNC_BYTE2[0]: 45 | raise PyRPlidarProtocolError("PyRPlidar Error : sync bytes are mismatched", hex(discriptor.sync_byte1), hex(discriptor.sync_byte2)) 46 | return discriptor 47 | 48 | def receive_data(self, discriptor): 49 | if self.lidar_serial == None: 50 | raise PyRPlidarConnectionError("PyRPlidar Error : received data length is mismatched") 51 | 52 | data = self.lidar_serial.receive_data(discriptor.data_length) 53 | if len(data) != discriptor.data_length: 54 | raise PyRPlidarProtocolError() 55 | return data 56 | 57 | 58 | 59 | def stop(self): 60 | self.send_command(RPLIDAR_CMD_STOP) 61 | 62 | def reset(self): 63 | self.send_command(RPLIDAR_CMD_RESET) 64 | 65 | def set_motor_pwm(self, pwm): 66 | self.lidar_serial.set_dtr(False) 67 | self.send_command(RPLIDAR_CMD_SET_MOTOR_PWM, struct.pack("> 30 115 | self.data_type = raw_bytes[6] 116 | 117 | def __str__(self): 118 | data = { 119 | "sync_byte1" : hex(self.sync_byte1), 120 | "sync_byte2" : hex(self.sync_byte2), 121 | "data_length" : self.data_length, 122 | "send_mode" : self.send_mode, 123 | "data_type" : hex(self.data_type) 124 | } 125 | return str(data) 126 | 127 | 128 | 129 | 130 | 131 | 132 | class PyRPlidarDeviceInfo: 133 | 134 | def __init__(self, raw_bytes): 135 | self.model = raw_bytes[0] 136 | self.firmware_minor = raw_bytes[1] 137 | self.firmware_major = raw_bytes[2] 138 | self.hardware = raw_bytes[3] 139 | self.serialnumber = codecs.encode(raw_bytes[4:], 'hex').upper() 140 | self.serialnumber = codecs.decode(self.serialnumber, 'ascii') 141 | 142 | def __str__(self): 143 | data = { 144 | "model" : self.model, 145 | "firmware_minor" : self.firmware_minor, 146 | "firmware_major" : self.firmware_major, 147 | "hardware" : self.hardware, 148 | "serialnumber" : self.serialnumber 149 | } 150 | return str(data) 151 | 152 | 153 | class PyRPlidarHealth: 154 | 155 | def __init__(self, raw_bytes): 156 | self.status = raw_bytes[0] 157 | self.error_code = (raw_bytes[1] << 8) + raw_bytes[2] 158 | 159 | def __str__(self): 160 | data = { 161 | "status" : self.status, 162 | "error_code" : self.error_code 163 | } 164 | return str(data) 165 | 166 | 167 | class PyRPlidarSamplerate: 168 | 169 | def __init__(self, raw_bytes): 170 | self.t_standard = raw_bytes[0] + (raw_bytes[1] << 8) 171 | self.t_express = raw_bytes[2] + (raw_bytes[3] << 8) 172 | 173 | def __str__(self): 174 | data = { 175 | "t_standard" : self.t_standard, 176 | "t_express" : self.t_express 177 | } 178 | return str(data) 179 | 180 | 181 | class PyRPlidarScanMode: 182 | 183 | def __init__(self, data_name, data_max_distance, data_us_per_sample, data_ans_type): 184 | self.us_per_sample = struct.unpack("> 2 209 | self.angle = ((raw_bytes[1] >> 1) + (raw_bytes[2] << 7)) / 64.0 210 | self.distance = (raw_bytes[3] + (raw_bytes[4] << 8)) / 4.0 211 | 212 | elif measurement_hq is not None: 213 | self.start_flag = True if measurement_hq.start_flag == 0x1 else False 214 | self.quality = measurement_hq.quality 215 | self.angle = ((measurement_hq.angle_z_q14 * 90) >> 8) / 64.0 216 | self.distance = (measurement_hq.dist_mm_q2) / 4.0 217 | 218 | 219 | def __str__(self): 220 | data = { 221 | "start_flag" : self.start_flag, 222 | "quality" : self.quality, 223 | "angle" : self.angle, 224 | "distance" : self.distance 225 | } 226 | return str(data) 227 | 228 | 229 | class PyRPlidarMeasurementHQ: 230 | 231 | def __init__(self, syncBit, angle_q6, dist_q2): 232 | self.start_flag = syncBit | ((not syncBit) << 1) 233 | self.quality = (0x2f << 2) if dist_q2 else 0 234 | self.angle_z_q14 = (angle_q6 << 8) // 90 235 | self.dist_mm_q2 = dist_q2 236 | 237 | def get_angle(self): 238 | return self.angle_z_q14 * 90.0 / 16384.0 239 | 240 | def get_distance(self): 241 | return self.dist_mm_q2 / 4.0 242 | 243 | 244 | 245 | 246 | class PyRPlidarCabin: 247 | 248 | def __init__(self, raw_bytes): 249 | self.distance1 = (raw_bytes[0] >> 2) + (raw_bytes[1] << 6) 250 | self.distance2 = (raw_bytes[2] >> 2) + (raw_bytes[3] << 6) 251 | self.d_theta1 = (raw_bytes[4] & 0x0F) + ((raw_bytes[0] & 0x03) << 4) 252 | self.d_theta2 = (raw_bytes[4] >> 4) + ((raw_bytes[2] & 0x03) << 4) 253 | 254 | class PyRPlidarScanCapsule: 255 | 256 | def __init__(self, raw_bytes): 257 | self.sync_byte1 = (raw_bytes[0] >> 4) & 0xF 258 | self.sync_byte2 = (raw_bytes[1] >> 4) & 0xF 259 | self.checksum = (raw_bytes[0] & 0xF) + ((raw_bytes[1] & 0xF) << 4) 260 | self.start_angle_q6 = raw_bytes[2] + ((raw_bytes[3] & 0x7F) << 8) 261 | self.start_flag = bool((raw_bytes[3] >> 7) & 0x1) 262 | self.cabins = list(map( 263 | PyRPlidarCabin, 264 | [raw_bytes[i:i+5] for i in range(4, len(raw_bytes), 5)] 265 | )) 266 | 267 | @classmethod 268 | def _parse_capsule(self, capsule_prev, capsule_current): 269 | 270 | nodes = [] 271 | 272 | currentStartAngle_q8 = capsule_current.start_angle_q6 << 2 273 | prevStartAngle_q8 = capsule_prev.start_angle_q6 << 2 274 | 275 | diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8) 276 | if prevStartAngle_q8 > currentStartAngle_q8: 277 | diffAngle_q8 += (360 << 8) 278 | 279 | angleInc_q16 = (diffAngle_q8 << 3) 280 | currentAngle_raw_q16 = (prevStartAngle_q8 << 8) 281 | 282 | for pos in range(len(capsule_prev.cabins)): 283 | 284 | dist_q2 = [0] * 2 285 | angle_q6 = [0] * 2 286 | syncBit = [0] * 2 287 | 288 | dist_q2[0] = capsule_prev.cabins[pos].distance1 << 2 289 | dist_q2[1] = capsule_prev.cabins[pos].distance2 << 2 290 | 291 | angle_offset1_q3 = capsule_prev.cabins[pos].d_theta1 292 | angle_offset2_q3 = capsule_prev.cabins[pos].d_theta2 293 | 294 | angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10) 295 | syncBit[0] = 1 if ((currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 else 0 296 | currentAngle_raw_q16 += angleInc_q16 297 | 298 | 299 | angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10) 300 | syncBit[1] = 1 if ((currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 else 0 301 | currentAngle_raw_q16 += angleInc_q16 302 | 303 | 304 | for cpos in range(2): 305 | 306 | if angle_q6[cpos] < 0: angle_q6[cpos] += (360 << 6) 307 | if angle_q6[cpos] >= (360 << 6): angle_q6[cpos] -= (360 << 6) 308 | 309 | node = PyRPlidarMeasurementHQ(syncBit[cpos], angle_q6[cpos], dist_q2[cpos]) 310 | nodes.append(node) 311 | 312 | return nodes 313 | 314 | 315 | 316 | 317 | 318 | 319 | class PyRPlidarDenseCabin: 320 | 321 | def __init__(self, raw_bytes): 322 | self.distance = (raw_bytes[0] << 8) + raw_bytes[1] 323 | 324 | 325 | class PyRPlidarScanDenseCapsule: 326 | 327 | def __init__(self, raw_bytes): 328 | self.sync_byte1 = (raw_bytes[0] >> 4) & 0xF 329 | self.sync_byte2 = (raw_bytes[1] >> 4) & 0xF 330 | self.checksum = (raw_bytes[0] & 0xF) + ((raw_bytes[1] & 0xF) << 4) 331 | self.start_angle_q6 = raw_bytes[2] + ((raw_bytes[3] & 0x7F) << 8) 332 | self.start_flag = bool((raw_bytes[3] >> 7) & 0x1) 333 | self.cabins = list(map( 334 | PyRPlidarDenseCabin, 335 | [raw_bytes[i:i+2] for i in range(4, len(raw_bytes), 2)] 336 | )) 337 | 338 | @classmethod 339 | def _parse_capsule(self, capsule_prev, capsule_current): 340 | 341 | nodes = [] 342 | 343 | currentStartAngle_q8 = capsule_current.start_angle_q6 << 2 344 | prevStartAngle_q8 = capsule_prev.start_angle_q6 << 2 345 | 346 | diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8) 347 | if prevStartAngle_q8 > currentStartAngle_q8: 348 | diffAngle_q8 += (360 << 8) 349 | 350 | angleInc_q16 = (diffAngle_q8 << 8) // 40 351 | currentAngle_raw_q16 = (prevStartAngle_q8 << 8) 352 | 353 | for pos in range(len(capsule_prev.cabins)): 354 | 355 | dist_q2 = 0 356 | angle_q6 = 0 357 | syncBit = 0 358 | 359 | syncBit = 1 if (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) else 0 360 | 361 | angle_q6 = (currentAngle_raw_q16 >> 10) 362 | if angle_q6 < 0: angle_q6 += (360 << 6) 363 | if angle_q6 >= (360 << 6): angle_q6 -= (360 << 6) 364 | currentAngle_raw_q16 += angleInc_q16 365 | 366 | dist_q2 = capsule_prev.cabins[pos].distance << 2 367 | 368 | node = PyRPlidarMeasurementHQ(syncBit, angle_q6, dist_q2) 369 | nodes.append(node) 370 | 371 | return nodes 372 | 373 | 374 | 375 | 376 | 377 | 378 | class PyRPlidarUltraCabin: 379 | 380 | def __init__(self, raw_bytes): 381 | self.major = ((int(raw_bytes[1]) & 0xF) << 8) + int(raw_bytes[0]) 382 | self.predict1 = ((int(raw_bytes[2]) & 0x3F) << 4) + ((int(raw_bytes[1]) >> 4) & 0xF) 383 | self.predict2 = ((int(raw_bytes[3]) & 0xFF) << 2) + ((int(raw_bytes[2]) >> 6) & 0x3) 384 | 385 | if self.predict1 & 0x200: self.predict1 |= 0xFFFFFC00 386 | if self.predict2 & 0x200: self.predict2 |= 0xFFFFFC00 387 | 388 | def __str__(self): 389 | data = { 390 | "major" : hex(self.major), 391 | "predict1" : hex(self.predict1), 392 | "predict2" : hex(self.predict2), 393 | } 394 | return str(data) 395 | 396 | class PyRPlidarScanUltraCapsule: 397 | 398 | def __init__(self, raw_bytes): 399 | self.sync_byte1 = (raw_bytes[0] >> 4) & 0xF 400 | self.sync_byte2 = (raw_bytes[1] >> 4) & 0xF 401 | self.checksum = (raw_bytes[0] & 0xF) + ((raw_bytes[1] & 0xF) << 4) 402 | self.start_angle_q6 = raw_bytes[2] + ((raw_bytes[3] & 0x7F) << 8) 403 | self.start_flag = bool((raw_bytes[3] >> 7) & 0x1) 404 | self.ultra_cabins = list(map( 405 | PyRPlidarUltraCabin, 406 | [raw_bytes[i:i+4] for i in range(4, len(raw_bytes), 4)] 407 | )) 408 | 409 | 410 | def __str__(self): 411 | data = { 412 | "sync_byte1" : hex(self.sync_byte1), 413 | "sync_byte2" : hex(self.sync_byte2), 414 | "checksum" : hex(self.checksum), 415 | "start_angle_q6" : hex(self.start_angle_q6), 416 | "start_flag" : self.start_flag, 417 | "ultra_cabins" : [str(ultra_cabin) for ultra_cabin in self.ultra_cabins] 418 | } 419 | return str(data) 420 | 421 | @classmethod 422 | def _varbitscale_decode(self, scaled): 423 | 424 | scaleLevel = 0 425 | 426 | for i in range(len(VBS_SCALED_BASE)): 427 | 428 | remain = scaled - VBS_SCALED_BASE[i] 429 | if remain >= 0: 430 | scaleLevel = VBS_SCALED_LVL[i] 431 | return (VBS_TARGET_BASE[i] + (remain << scaleLevel), scaleLevel) 432 | 433 | return (0, scaleLevel) 434 | 435 | @classmethod 436 | def _parse_capsule(self, capsule_prev, capsule_current): 437 | 438 | nodes = [] 439 | 440 | currentStartAngle_q8 = capsule_current.start_angle_q6 << 2 441 | prevStartAngle_q8 = capsule_prev.start_angle_q6 << 2 442 | 443 | diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8) 444 | if prevStartAngle_q8 > currentStartAngle_q8: 445 | diffAngle_q8 += (360 << 8) 446 | 447 | angleInc_q16 = (diffAngle_q8 << 3) // 3 448 | currentAngle_raw_q16 = (prevStartAngle_q8 << 8) 449 | 450 | for pos in range(len(capsule_prev.ultra_cabins)): 451 | 452 | dist_q2 = [0] * 3 453 | angle_q6 = [0] * 3 454 | syncBit = [0] * 3 455 | 456 | dist_major = capsule_prev.ultra_cabins[pos].major 457 | 458 | # signed partical integer, using the magic shift here 459 | # DO NOT TOUCH 460 | 461 | dist_predict1 = capsule_prev.ultra_cabins[pos].predict1 462 | dist_predict2 = capsule_prev.ultra_cabins[pos].predict2 463 | 464 | dist_major2 = 0 465 | 466 | # prefetch next ... 467 | if pos == len(capsule_prev.ultra_cabins) - 1: 468 | dist_major2 = capsule_current.ultra_cabins[0].major 469 | else: 470 | dist_major2 = capsule_prev.ultra_cabins[pos + 1].major 471 | 472 | 473 | # decode with the var bit scale ... 474 | dist_major, scalelvl1 = PyRPlidarScanUltraCapsule._varbitscale_decode(dist_major) 475 | dist_major2, scalelvl2 = PyRPlidarScanUltraCapsule._varbitscale_decode(dist_major2) 476 | 477 | 478 | dist_base1 = dist_major 479 | dist_base2 = dist_major2 480 | 481 | if not(dist_major) and dist_major2: 482 | dist_base1 = dist_major2 483 | scalelvl1 = scalelvl2 484 | 485 | dist_q2[0] = (dist_major << 2) 486 | if (dist_predict1 == 0xFFFFFE00) or (dist_predict1 == 0x1FF): 487 | dist_q2[1] = 0 488 | else: 489 | dist_predict1 = (dist_predict1 << scalelvl1) 490 | dist_q2[1] = ((dist_predict1 + dist_base1) << 2) & 0xFFFFFFFF 491 | 492 | if (dist_predict2 == 0xFFFFFE00) or (dist_predict2 == 0x1FF): 493 | dist_q2[2] = 0 494 | else: 495 | dist_predict2 = (dist_predict2 << scalelvl2) 496 | dist_q2[2] = ((dist_predict2 + dist_base2) << 2) & 0xFFFFFFFF 497 | 498 | 499 | for cpos in range(3): 500 | 501 | syncBit[cpos] = 1 if (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) else 0 502 | 503 | offsetAngleMean_q16 = int(7.5 * 3.1415926535 * (1 << 16) / 180.0) 504 | 505 | if dist_q2[cpos] >= (50 * 4): 506 | 507 | k1 = 98361 508 | k2 = int(k1 / dist_q2[cpos]) 509 | 510 | offsetAngleMean_q16 = int(8 * 3.1415926535 * (1 << 16) / 180) - int(k2 << 6) - int((k2 * k2 * k2) / 98304) 511 | 512 | angle_q6[cpos] = (currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10 513 | currentAngle_raw_q16 += angleInc_q16 514 | 515 | if angle_q6[cpos] < 0: angle_q6[cpos] += (360 << 6) 516 | if angle_q6[cpos] >= (360 << 6): angle_q6[cpos] -= (360 << 6) 517 | 518 | node = PyRPlidarMeasurementHQ(syncBit[cpos], angle_q6[cpos], dist_q2[cpos]) 519 | nodes.append(node) 520 | 521 | return nodes 522 | -------------------------------------------------------------------------------- /test/test_pyrplidar_protocol.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import time 3 | 4 | import pyrplidar_protocol 5 | from pyrplidar_protocol import * 6 | 7 | 8 | class PyRPlidarProtocolTest(unittest.TestCase): 9 | 10 | def test_stop_req_packet(self): 11 | cmd = PyRPlidarCommand(RPLIDAR_CMD_STOP) 12 | self.assertEqual(cmd.raw_bytes, b"\xA5\x25") 13 | 14 | def test_reset_req_packet(self): 15 | cmd = PyRPlidarCommand(RPLIDAR_CMD_RESET) 16 | self.assertEqual(cmd.raw_bytes, b"\xA5\x40") 17 | 18 | def test_get_info_req_packet(self): 19 | cmd = PyRPlidarCommand(RPLIDAR_CMD_GET_INFO) 20 | self.assertEqual(cmd.raw_bytes, b"\xA5\x50") 21 | 22 | def test_parse_descriptor_01(self): 23 | descriptor = PyRPlidarResponse(b"\xA5\x5A\x04\x00\x00\x00\x15") 24 | self.assertEqual(descriptor.data_length, 0x04) 25 | self.assertEqual(descriptor.send_mode, 0) 26 | self.assertEqual(descriptor.data_type, 0x15) 27 | 28 | def test_parse_descriptor_02(self): 29 | descriptor = PyRPlidarResponse(b"\xA5\x5A\x84\x00\x00\x40\x84") 30 | self.assertEqual(descriptor.data_length, 0x84) 31 | self.assertEqual(descriptor.send_mode, 1) 32 | self.assertEqual(descriptor.data_type, 0x84) 33 | 34 | 35 | def test_varbitscale_decode(self): 36 | 37 | dist_major_input = [0x1E0, 0x20B, 0x219, 0x504, 0x507, 0x51E] 38 | dist_major_output = [0x1E0, 0x216, 0x232, 0x810, 0x81C, 0x878] 39 | scalelvl_output = [0, 1, 1 ,2, 2, 2] 40 | 41 | for i in range(len(dist_major_input)): 42 | dist_major, scalelvl = PyRPlidarScanUltraCapsule._varbitscale_decode(dist_major_input[i]) 43 | self.assertEqual(dist_major, dist_major_output[i]) 44 | self.assertEqual(scalelvl, scalelvl_output[i]) 45 | 46 | 47 | def test_capsule_parsing(self): 48 | 49 | nodes_result = [ 50 | [[0x2, 0xBC, 0xE282, 0x97C], [0x2, 0xBC, 0xE2EE, 0x970], [0x2, 0xBC, 0xE32D, 0x964], [0x2, 0xBC, 0xE3B0, 0x958], [0x2, 0xBC, 0xE3F1, 0x964], [0x2, 0xBC, 0xE45D, 0x96C], [0x2, 0xBC, 0xE4B3, 0x960], [0x2, 0xBC, 0xE4DB, 0x960], [0x2, 0xBC, 0xE549, 0x95C], [0x2, 0xBC, 0xE59F, 0x958], [0x2, 0xBC, 0xE60B, 0x94C], [0x2, 0xBC, 0xE64C, 0x958], [0x2, 0xBC, 0xE6A2, 0x960], [0x2, 0xBC, 0xE70E, 0x954], [0x2, 0xBC, 0xE763, 0x954], [0x2, 0xBC, 0xE7A4, 0x94C], [0x2, 0xBC, 0xE7FA, 0x93C], [0x2, 0xBC, 0xE84F, 0x938], [0x2, 0xBC, 0xE8A7, 0x938], [0x2, 0xBC, 0xE913, 0x938], [0x2, 0xBC, 0xE969, 0x934], [0x2, 0xBC, 0xE9BE, 0x934], [0x2, 0xBC, 0xE9E9, 0x934], [0x2, 0xBC, 0xEA55, 0x934], [0x2, 0xBC, 0xEAC1, 0x934], [0x2, 0xBC, 0xEB02, 0x938], [0x2, 0xBC, 0xEB58, 0x938], [0x2, 0xBC, 0xEBC4, 0x940], [0x2, 0xBC, 0xEC19, 0x94C], [0x2, 0xBC, 0xEC44, 0x948], [0x2, 0xBC, 0xECB0, 0x954], [0x2, 0xBC, 0xED05, 0x968]], 51 | [[0x2, 0xBC, 0xED47, 0x968], [0x2, 0xBC, 0xED99, 0x970], [0x2, 0xBC, 0xEE05, 0x974], [0x2, 0xBC, 0xEE58, 0x968], [0x2, 0xBC, 0xEE96, 0x964], [0x2, 0xBC, 0xEF19, 0x960], [0x2, 0xBC, 0xEF55, 0x958], [0x2, 0xBC, 0xEFC1, 0x958], [0x2, 0xBC, 0xF016, 0x95C], [0x2, 0xBC, 0xF03B, 0x96C], [0x2, 0xBC, 0xF091, 0x978], [0x2, 0xBC, 0xF0E3, 0x97C], [0x2, 0xBC, 0xF14F, 0x980], [0x2, 0xBC, 0xF1A4, 0x988], [0x2, 0xBC, 0xF1E0, 0x98C], [0x2, 0xBC, 0xF24C, 0x998], [0x2, 0xBC, 0xF28B, 0x9A0], [0x2, 0xBC, 0xF2DD, 0x9AC], [0x2, 0xBC, 0xF333, 0x9B0], [0x2, 0xBC, 0xF385, 0x9AC], [0x2, 0xBC, 0xF3DB, 0x9B0], [0x2, 0xBC, 0xF430, 0x9B4], [0x2, 0xBC, 0xF482, 0x9B8], [0x2, 0xBC, 0xF4C1, 0x9BC], [0x2, 0xBC, 0xF52D, 0x9C4], [0x2, 0xBC, 0xF569, 0x9C8], [0x2, 0xBC, 0xF5BE, 0x9D0], [0x2, 0xBC, 0xF627, 0x9D0], [0x2, 0xBC, 0xF666, 0x9D4], [0x2, 0xBC, 0xF6E9, 0x9DC], [0x2, 0xBC, 0xF724, 0x9E8], [0x2, 0xBC, 0xF77A, 0x9F4]], 52 | [[0x2, 0xBC, 0xF7CF, 0x9F4], [0x2, 0xBC, 0xF80B, 0xA04], [0x2, 0xBC, 0xF860, 0xA1C], [0x2, 0xBC, 0xF8B3, 0xA24], [0x2, 0xBC, 0xF908, 0xA2C], [0x2, 0xBC, 0xF95D, 0xA30], [0x2, 0xBC, 0xF9B0, 0xA30], [0x2, 0xBC, 0xFA05, 0xA34], [0x2, 0xBC, 0xFA71, 0xA38], [0x2, 0xBC, 0xFAC4, 0xA40], [0x2, 0xBC, 0xFB02, 0xA4C], [0x2, 0xBC, 0xFB6C, 0xA54], [0x1, 0xBC, 0xFBAA, 0xA5C], [0x2, 0xBC, 0xFC00, 0xA68], [0x2, 0xBC, 0xFC52, 0xA78], [0x2, 0xBC, 0xFC91, 0xA80], [0x2, 0xBC, 0xFCE6, 0xA90], [0x2, 0xBC, 0xFD4F, 0xA9C], [0x2, 0xBC, 0xFDA4, 0xA9C], [0x2, 0xBC, 0xFDF7, 0xAA0], [0x2, 0xBC, 0xFE36, 0xAA4], [0x2, 0xBC, 0xFEA2, 0xAA4], [0x2, 0xBC, 0xFEF4, 0xA9C], [0x2, 0xBC, 0xFF49, 0xA98], [0x2, 0xBC, 0xFF88, 0xA94], [0x2, 0xBC, 0xFFF1, 0xA94], [0x2, 0xBC, 0x30, 0xA98], [0x2, 0xBC, 0x82, 0xAA0], [0x2, 0xBC, 0xD8, 0xAAC], [0x2, 0xBC, 0x12D, 0xAB4], [0x2, 0xBC, 0x196, 0xAC0], [0x2, 0xBC, 0x1D5, 0xACC]], 53 | [[0x2, 0xBC, 0x22A, 0xAD4], [0x2, 0xBC, 0x27D, 0xADC], [0x2, 0xBC, 0x2E9, 0xAF0], [0x2, 0xBC, 0x324, 0xAF4], [0x2, 0xBC, 0x37A, 0xAE0], [0x2, 0xBC, 0x3E6, 0xAE4], [0x2, 0xBC, 0x438, 0xAE4], [0x2, 0xBC, 0x477, 0xAF0], [0x2, 0x0, 0x4E3, 0x0], [0x2, 0xBC, 0x536, 0xA88], [0x2, 0xBC, 0x58B, 0xA64], [0x2, 0xBC, 0x5C7, 0xA64], [0x2, 0xBC, 0x633, 0xA64], [0x2, 0xBC, 0x688, 0xA60], [0x2, 0xBC, 0x6DB, 0xA60], [0x2, 0x0, 0x730, 0x0], [0x2, 0xBC, 0x785, 0x9E8], [0x2, 0xBC, 0x7D8, 0xA24], [0x2, 0x0, 0xC5B, 0x0], [0x2, 0x0, 0xCAD, 0x0], [0x2, 0x0, 0x8BE, 0x0], [0x2, 0xBC, 0x913, 0xAEC], [0x2, 0xBC, 0x94F, 0xAF0], [0x2, 0xBC, 0x9BB, 0xAF4], [0x2, 0xBC, 0x9FA, 0xAFC], [0x2, 0xBC, 0xA63, 0xB04], [0x2, 0xBC, 0xAB8, 0xB14], [0x2, 0xBC, 0xB0B, 0xB28], [0x2, 0xBC, 0xB60, 0xB44], [0x2, 0xBC, 0xB9F, 0xB64], [0x2, 0xBC, 0xBF1, 0xB80], [0x2, 0xBC, 0xC30, 0xBB8]]] 54 | 55 | data = [b"\xAC\x50\x12\x51\x7E\x09\x72\x09\xDE\x66\x09\x5A\x09\xCE\x66\x09\x6E\x09\xCD\x62\x09\x62\x09\xEC\x5E\x09\x5A\x09\xDD\x4E\x09\x5A\x09\xDC\x62\x09\x56\x09\xCD\x56\x09\x4E\x09\xDC\x3E\x09\x3A\x09\xDD\x3A\x09\x3A\x09\xCD\x36\x09\x36\x09\xCC\x36\x09\x36\x09\xDE\x36\x09\x3A\x09\xDC\x3A\x09\x42\x09\xCD\x4E\x09\x4A\x09\xEC\x56\x09\x6A\x09\xDD", b"\xAE\x5A\xDB\x54\x6A\x09\x72\x09\xEE\x76\x09\x6A\x09\xDD\x66\x09\x62\x09\xCE\x5A\x09\x5A\x09\xCD\x5E\x09\x6E\x09\xEC\x7A\x09\x7E\x09\xEE\x82\x09\x8A\x09\xDD\x8E\x09\x9A\x09\xDE\xA2\x09\xAE\x09\xEE\xB2\x09\xAE\x09\xEE\xB2\x09\xB6\x09\xEE\xBA\x09\xBE\x09\xFE\xC6\x09\xCA\x09\xFE\xD2\x09\xD2\x09\xEF\xD6\x09\xDE\x09\xDF\xEA\x09\xF6\x09\xEE", b"\xAE\x5B\x8F\x58\xF6\x09\x06\x0A\xFE\x1E\x0A\x26\x0A\xFF\x2E\x0A\x32\x0A\xFF\x32\x0A\x36\x0A\xFF\x3A\x0A\x42\x0A\xEE\x4E\x0A\x56\x0A\xEF\x5E\x0A\x6A\x0A\xFF\x7A\x0A\x83\x0A\x0F\x93\x0A\x9E\x0A\xF0\x9E\x0A\xA2\x0A\xFF\xA7\x0A\xA6\x0A\xF0\x9E\x0A\x9A\x0A\xFF\x97\x0A\x96\x0A\xF0\x9B\x0A\xA3\x0A\x00\xAF\x0A\xB7\x0A\x00\xC2\x0A\xCF\x0A\x0F", b"\xAF\x58\x43\x02\xD7\x0A\xDF\x0A\x00\xF2\x0A\xF7\x0A\x0F\xE3\x0A\xE6\x0A\xF0\xE6\x0A\xF3\x0A\x0F\x02\x00\x8A\x0A\xFF\x66\x0A\x67\x0A\x0F\x66\x0A\x62\x0A\xFF\x62\x0A\x02\x00\xFF\xEA\x09\x26\x0A\xFF\x00\x00\x00\x00\x00\x03\x00\xEF\x0A\x00\xF3\x0A\xF7\x0A\x01\xFF\x0A\x07\x0B\x01\x17\x0B\x2B\x0B\x00\x47\x0B\x67\x0B\x10\x83\x0B\xBB\x0B\x21", b"\xAB\x51\xF7\x05\x00\x00\x00\x00\x00\x03\x00\xD7\x0D\x33\x03\x0E\x13\x0E\x44\xFB\x0D\xC7\x0D\x33\x7B\x0D\x03\x00\x33\xC7\x0C\x8F\x0C\x23\x73\x0C\x67\x0C\x22\x5B\x0C\x5B\x0C\x21\x63\x0C\x6B\x0C\x21\x77\x0C\x8B\x0C\x22\xB3\x0C\xD3\x0C\x33\xEF\x0C\x17\x0D\x32\x53\x0D\x03\x00\x44\x27\x0E\x4B\x0E\x44\x53\x0E\x5B\x0E\x44\x5F\x0E\x6B\x0E\x44"] 56 | 57 | 58 | capsule_prev = PyRPlidarScanCapsule(data[0]) 59 | capsule_current = None 60 | 61 | for i in range(1,5): 62 | capsule_current = PyRPlidarScanCapsule(data[i]) 63 | 64 | nodes = PyRPlidarScanCapsule._parse_capsule(capsule_prev, capsule_current) 65 | 66 | for j in range(len(nodes)): 67 | start_flag, quality, angle_z_q14, dist_mm_q2 = nodes_result[i-1][j] 68 | self.assertEqual(start_flag, nodes[j].start_flag) 69 | self.assertEqual(quality, nodes[j].quality) 70 | self.assertEqual(angle_z_q14, nodes[j].angle_z_q14) 71 | self.assertEqual(dist_mm_q2, nodes[j].dist_mm_q2) 72 | 73 | capsule_prev = capsule_current 74 | 75 | 76 | 77 | def test_dense_capsule_parsing(self): 78 | pass 79 | 80 | 81 | def test_ultra_capsule_parsing(self): 82 | 83 | nodes_result = [ 84 | [[0x2, 0xBC, 0x331F, 0x454], [0x2, 0xBC, 0x3363, 0x458], [0x2, 0xBC, 0x33AA, 0x458], [0x2, 0xBC, 0x33EE, 0x458], [0x2, 0xBC, 0x3436, 0x458], [0x2, 0xBC, 0x347A, 0x454], [0x2, 0xBC, 0x34C1, 0x454], [0x2, 0xBC, 0x3505, 0x454], [0x2, 0xBC, 0x3549, 0x454], [0x2, 0xBC, 0x3591, 0x454], [0x2, 0xBC, 0x35D5, 0x454], [0x2, 0xBC, 0x361C, 0x454], [0x2, 0xBC, 0x3660, 0x454], [0x2, 0xBC, 0x36A7, 0x454], [0x2, 0xBC, 0x36EC, 0x454], [0x2, 0xBC, 0x373B, 0x450], [0x2, 0xBC, 0x378B, 0x444], [0x2, 0xBC, 0x37DB, 0x430], [0x2, 0xBC, 0x3836, 0x41C], [0x2, 0x0, 0x3522, 0x0], [0x2, 0x0, 0x3566, 0x0], [0x2, 0xBC, 0x3911, 0x40C], [0x2, 0x0, 0x35F1, 0x0], [0x2, 0x0, 0x3636, 0x0], [0x2, 0xBC, 0x3947, 0x4D0], [0x2, 0xBC, 0x3996, 0x4C4], [0x2, 0xBC, 0x39E6, 0x4B0], [0x2, 0xBC, 0x3A36, 0x4A8], [0x2, 0xBC, 0x3A7A, 0x4A4], [0x2, 0xBC, 0x3AC1, 0x4A4], [0x2, 0xBC, 0x3B05, 0x4A4], [0x2, 0xBC, 0x3B4C, 0x4A4], [0x2, 0xBC, 0x3B9C, 0x49C], [0x2, 0xBC, 0x3BE0, 0x49C], [0x2, 0xBC, 0x3C27, 0x49C], [0x2, 0xBC, 0x3C6C, 0x498], [0x2, 0xBC, 0x3CB0, 0x498], [0x2, 0xBC, 0x3CF7, 0x498], [0x2, 0xBC, 0x3D3B, 0x498], [0x2, 0xBC, 0x3D82, 0x498], [0x2, 0xBC, 0x3DC7, 0x498], [0x2, 0xBC, 0x3E0E, 0x498], [0x2, 0xBC, 0x3E52, 0x498], [0x2, 0xBC, 0x3E96, 0x498], [0x2, 0xBC, 0x3EDD, 0x498], [0x2, 0xBC, 0x3F22, 0x498], [0x2, 0xBC, 0x3F69, 0x49C], [0x2, 0xBC, 0x3FAD, 0x4A0], [0x2, 0xBC, 0x3FE9, 0x4A4], [0x2, 0xBC, 0x402D, 0x4A8], [0x2, 0xBC, 0x4074, 0x4AC], [0x2, 0xBC, 0x40B0, 0x4B0], [0x2, 0xBC, 0x40F4, 0x4B4], [0x2, 0xBC, 0x4138, 0x4BC], [0x2, 0xBC, 0x4174, 0x4C8], [0x2, 0xBC, 0x41B0, 0x4D0], [0x2, 0xBC, 0x41F7, 0x4D4], [0x2, 0xBC, 0x423B, 0x4DC], [0x2, 0xBC, 0x4277, 0x4E0], [0x2, 0xBC, 0x42BB, 0x4E4], [0x2, 0xBC, 0x4302, 0x4E4], [0x2, 0xBC, 0x4347, 0x4E4], [0x2, 0xBC, 0x438E, 0x4E4], [0x2, 0xBC, 0x43D2, 0x4E4], [0x2, 0xBC, 0x4416, 0x4E8], [0x2, 0x0, 0x419C, 0x0], [0x2, 0xBC, 0x44A2, 0x4EC], [0x2, 0xBC, 0x4511, 0x4A4], [0x2, 0xBC, 0x4560, 0x4A0], [0x2, 0xBC, 0x45A4, 0x4A0], [0x2, 0xBC, 0x45E0, 0x4A4], [0x2, 0xBC, 0x4627, 0x4A8], [0x2, 0xBC, 0x4663, 0x4B0], [0x2, 0x0, 0x43C9, 0x0], [0x2, 0x0, 0x440E, 0x0], [0x2, 0xBC, 0x4733, 0x4BC], [0x2, 0xBC, 0x4763, 0x4D4], [0x2, 0xBC, 0x479F, 0x4EC], [0x2, 0xBC, 0x47DB, 0x4F8], [0x2, 0xBC, 0x4816, 0x500], [0x2, 0xBC, 0x485B, 0x504], [0x2, 0xBC, 0x489F, 0x500], [0x2, 0xBC, 0x48EE, 0x4FC], [0x2, 0xBC, 0x4936, 0x4FC], [0x2, 0xBC, 0x4971, 0x500], [0x2, 0xBC, 0x49B6, 0x500], [0x2, 0x0, 0x474F, 0x0], [0x2, 0xBC, 0x4A41, 0x504], [0x2, 0xBC, 0x4AA4, 0x4D8], [0x2, 0xBC, 0x4AEC, 0x4D0], [0x2, 0xBC, 0x4B3B, 0x4C4], [0x2, 0xBC, 0x4B8B, 0x4BC], [0x2, 0xBC, 0x4BCF, 0x4B8], [0x2, 0xBC, 0x4C1F, 0x4AC], [0x2, 0xBC, 0x4C66, 0x4A8], [0x2, 0xBC, 0x4CAA, 0x4A4]], 85 | [[0x2, 0xBC, 0x4CEE, 0x4A4], [0x2, 0xBC, 0x4D2A, 0x4B4], [0x2, 0xBC, 0x4D66, 0x4C8], [0x2, 0xBC, 0x4DAA, 0x4CC], [0x2, 0xBC, 0x4DE3, 0x4D4], [0x2, 0xBC, 0x4E27, 0x4D8], [0x2, 0xBC, 0x4E6C, 0x4DC], [0x2, 0xBC, 0x4EA7, 0x4E8], [0x2, 0xBC, 0x4EE0, 0x4F0], [0x2, 0xBC, 0x4F27, 0x4F8], [0x2, 0x0, 0x4CB6, 0x0], [0x2, 0x0, 0x4CFA, 0x0], [0x2, 0xBC, 0x4FE9, 0x500], [0x2, 0x0, 0x4D82, 0x0], [0x2, 0x0, 0x4DC7, 0x0], [0x2, 0xBC, 0x5085, 0x568], [0x2, 0xBC, 0x50BE, 0x570], [0x2, 0xBC, 0x5102, 0x57C], [0x2, 0xBC, 0x513E, 0x580], [0x2, 0xBC, 0x5182, 0x580], [0x2, 0xBC, 0x51C7, 0x584], [0x2, 0xBC, 0x520B, 0x588], [0x2, 0xBC, 0x5252, 0x58C], [0x2, 0xBC, 0x5296, 0x590], [0x2, 0xBC, 0x52CF, 0x594], [0x2, 0xBC, 0x5313, 0x598], [0x2, 0xBC, 0x5358, 0x5A0], [0x2, 0xBC, 0x5393, 0x5AC], [0x2, 0xBC, 0x53CC, 0x5C4], [0x2, 0xBC, 0x5408, 0x5D8], [0x2, 0xBC, 0x544C, 0x5E0], [0x2, 0xBC, 0x5491, 0x5E4], [0x2, 0xBC, 0x54CC, 0x5F4], [0x2, 0xBC, 0x5505, 0x608], [0x2, 0xBC, 0x5549, 0x618], [0x2, 0xBC, 0x5585, 0x624], [0x2, 0xBC, 0x55C9, 0x630], [0x2, 0xBC, 0x5605, 0x638], [0x2, 0xBC, 0x5649, 0x640], [0x2, 0xBC, 0x568E, 0x644], [0x2, 0xBC, 0x56D2, 0x64C], [0x2, 0xBC, 0x570E, 0x658], [0x2, 0xBC, 0x5752, 0x664], [0x2, 0xBC, 0x578B, 0x66C], [0x2, 0x0, 0x55D2, 0x0], [0x2, 0xBC, 0x5833, 0x628], [0x2, 0xBC, 0x5877, 0x620], [0x2, 0xBC, 0x58BE, 0x620], [0x2, 0xBC, 0x590B, 0x618], [0x2, 0xBC, 0x5947, 0x624], [0x2, 0xBC, 0x5980, 0x634], [0x2, 0xBC, 0x59CF, 0x630], [0x2, 0xBC, 0x5A13, 0x628], [0x2, 0xBC, 0x5A58, 0x624], [0x2, 0xBC, 0x5A9C, 0x624], [0x2, 0xBC, 0x5AE3, 0x624], [0x2, 0xBC, 0x5B27, 0x624], [0x2, 0xBC, 0x5B6C, 0x624], [0x2, 0xBC, 0x5BB0, 0x620], [0x2, 0xBC, 0x5BF4, 0x620], [0x2, 0xBC, 0x5C38, 0x620], [0x2, 0xBC, 0x5C7D, 0x620], [0x2, 0xBC, 0x5CC1, 0x624], [0x2, 0xBC, 0x5D08, 0x624], [0x2, 0xBC, 0x5D4C, 0x62C], [0x2, 0x0, 0x5B74, 0x0], [0x2, 0xBC, 0x5DC9, 0x634], [0x2, 0x0, 0x5BFD, 0x0], [0x2, 0x0, 0x5C41, 0x0], [0x2, 0xBC, 0x5E6E, 0x6B4], [0x2, 0xBC, 0x5EAA, 0x6C0], [0x2, 0xBC, 0x5EEE, 0x6D0], [0x2, 0xBC, 0x5F33, 0x6D8], [0x2, 0xBC, 0x5F6E, 0x6E0], [0x2, 0xBC, 0x5FB3, 0x6E8], [0x2, 0xBC, 0x5FF7, 0x6F0], [0x2, 0xBC, 0x603B, 0x6F4], [0x2, 0xBC, 0x6077, 0x700], [0x2, 0xBC, 0x60BB, 0x704], [0x2, 0xBC, 0x6100, 0x70C], [0x2, 0xBC, 0x6144, 0x710], [0x2, 0xBC, 0x6188, 0x710], [0x2, 0xBC, 0x61CC, 0x710], [0x2, 0xBC, 0x6211, 0x710], [0x2, 0xBC, 0x6255, 0x710], [0x2, 0xBC, 0x629C, 0x70C], [0x2, 0xBC, 0x62E0, 0x70C], [0x2, 0xBC, 0x6324, 0x70C], [0x2, 0xBC, 0x6369, 0x710], [0x2, 0xBC, 0x63AD, 0x714], [0x2, 0xBC, 0x63F1, 0x718], [0x2, 0xBC, 0x6436, 0x71C], [0x2, 0x0, 0x62B0, 0x0], [0x2, 0xBC, 0x64B6, 0x728], [0x2, 0x0, 0x6338, 0x0], [0x2, 0x0, 0x637D, 0x0]], 86 | [[0x2, 0x0, 0x63C4, 0x0], [0x2, 0x0, 0x6408, 0x0], [0x2, 0x0, 0x644C, 0x0], [0x2, 0x0, 0x6493, 0x0], [0x2, 0x0, 0x64D8, 0x0], [0x2, 0x0, 0x651F, 0x0], [0x2, 0x0, 0x6563, 0x0], [0x2, 0x0, 0x65A7, 0x0], [0x2, 0x0, 0x65EE, 0x0], [0x2, 0xBC, 0x6666, 0x1AA0], [0x2, 0xBC, 0x66AD, 0x1A60], [0x2, 0xBC, 0x66F1, 0x1A38], [0x2, 0xBC, 0x6736, 0x1A18], [0x2, 0xBC, 0x677D, 0x19F0], [0x2, 0xBC, 0x67C1, 0x19D0], [0x2, 0xBC, 0x6808, 0x19A8], [0x2, 0xBC, 0x6858, 0x1990], [0x2, 0xBC, 0x689C, 0x1980], [0x2, 0xBC, 0x68E0, 0x1958], [0x2, 0xBC, 0x6927, 0x1930], [0x2, 0xBC, 0x696C, 0x1900], [0x2, 0xBC, 0x69B3, 0x18D8], [0x2, 0xBC, 0x69F7, 0x18C0], [0x2, 0xBC, 0x6A3B, 0x18B0], [0x2, 0xBC, 0x6A82, 0x18A0], [0x2, 0xBC, 0x6AC7, 0x1888], [0x2, 0xBC, 0x6B0E, 0x1870], [0x2, 0xBC, 0x6B52, 0x1858], [0x2, 0xBC, 0x6B96, 0x1838], [0x2, 0xBC, 0x6BDD, 0x1818], [0x2, 0xBC, 0x6C2D, 0x17F8], [0x2, 0xBC, 0x6C71, 0x17E8], [0x2, 0xBC, 0x6CB6, 0x17D0], [0x2, 0xBC, 0x6CFD, 0x17B8], [0x2, 0xBC, 0x6D41, 0x17A0], [0x2, 0xBC, 0x6D88, 0x1790], [0x2, 0xBC, 0x6DCC, 0x1778], [0x2, 0xBC, 0x6E11, 0x1760], [0x2, 0xBC, 0x6E58, 0x1750], [0x2, 0xBC, 0x6E9C, 0x1728], [0x2, 0xBC, 0x6EE3, 0x1710], [0x2, 0xBC, 0x6F27, 0x1710], [0x2, 0xBC, 0x6F6C, 0x1700], [0x2, 0xBC, 0x6FB3, 0x16E0], [0x2, 0xBC, 0x6FF7, 0x16C8], [0x2, 0xBC, 0x703E, 0x16C0], [0x2, 0xBC, 0x7082, 0x16B0], [0x2, 0xBC, 0x70C7, 0x16A0], [0x2, 0xBC, 0x7116, 0x1690], [0x2, 0xBC, 0x715D, 0x1678], [0x2, 0xBC, 0x71A2, 0x1660], [0x2, 0xBC, 0x71E9, 0x1650], [0x2, 0xBC, 0x722D, 0x1640], [0x2, 0xBC, 0x7271, 0x1638], [0x2, 0xBC, 0x72B8, 0x1630], [0x2, 0xBC, 0x72FD, 0x1620], [0x2, 0xBC, 0x7344, 0x1610], [0x2, 0xBC, 0x7388, 0x1600], [0x2, 0xBC, 0x73CC, 0x15F0], [0x2, 0xBC, 0x7413, 0x15E0], [0x2, 0xBC, 0x7458, 0x15D0], [0x2, 0xBC, 0x749F, 0x15C0], [0x2, 0xBC, 0x74E3, 0x15B0], [0x2, 0xBC, 0x7527, 0x15A0], [0x2, 0xBC, 0x756E, 0x1598], [0x2, 0xBC, 0x75B3, 0x1588], [0x2, 0xBC, 0x75FA, 0x1580], [0x2, 0xBC, 0x763E, 0x1578], [0x2, 0xBC, 0x7682, 0x1570], [0x2, 0xBC, 0x76C9, 0x1568], [0x2, 0xBC, 0x770E, 0x1560], [0x2, 0xBC, 0x775D, 0x1550], [0x2, 0xBC, 0x77A2, 0x1550], [0x2, 0xBC, 0x77E9, 0x1548], [0x2, 0xBC, 0x782D, 0x1538], [0x2, 0xBC, 0x7874, 0x1528], [0x2, 0xBC, 0x78B8, 0x1520], [0x2, 0xBC, 0x78FD, 0x1518], [0x2, 0xBC, 0x7944, 0x1510], [0x2, 0xBC, 0x7988, 0x1508], [0x2, 0xBC, 0x79CF, 0x1500], [0x2, 0xBC, 0x7A13, 0x14F8], [0x2, 0xBC, 0x7A58, 0x14F8], [0x2, 0xBC, 0x7A9F, 0x14F0], [0x2, 0xBC, 0x7AE3, 0x14E8], [0x2, 0xBC, 0x7B2A, 0x14E0], [0x2, 0xBC, 0x7B6E, 0x14D8], [0x2, 0xBC, 0x7BB3, 0x14D0], [0x2, 0xBC, 0x7BFA, 0x14D8], [0x2, 0x0, 0x7BE3, 0x0], [0x2, 0xBC, 0x7C85, 0x14D0], [0x2, 0x0, 0x7C6E, 0x0], [0x2, 0xBC, 0x7E2D, 0x858], [0x2, 0xBC, 0x7E71, 0x848], [0x2, 0xBC, 0x7EB6, 0x840], [0x2, 0xBC, 0x7EFD, 0x838]], 87 | [[0x2, 0xBC, 0x7F41, 0x838], [0x2, 0xBC, 0x7F88, 0x838], [0x2, 0xBC, 0x7FCC, 0x838], [0x2, 0xBC, 0x8011, 0x840], [0x2, 0xBC, 0x8055, 0x840], [0x2, 0xBC, 0x809C, 0x850], [0x2, 0xBC, 0x80E0, 0x858], [0x2, 0x0, 0x7FAD, 0x0], [0x2, 0x0, 0x7FF1, 0x0], [0x2, 0x0, 0x8036, 0x0], [0x2, 0x0, 0x807D, 0x0], [0x2, 0x0, 0x80C1, 0x0], [0x2, 0x0, 0x8105, 0x0], [0x2, 0x0, 0x8149, 0x0], [0x2, 0x0, 0x8191, 0x0], [0x2, 0xBC, 0x8233, 0x1470], [0x2, 0xBC, 0x8277, 0x1490], [0x2, 0xBC, 0x82BB, 0x1490], [0x2, 0xBC, 0x8300, 0x1490], [0x2, 0xBC, 0x8347, 0x1490], [0x2, 0xBC, 0x838B, 0x1490], [0x2, 0xBC, 0x83CF, 0x1490], [0x2, 0xBC, 0x8416, 0x1490], [0x2, 0xBC, 0x845B, 0x1490], [0x2, 0xBC, 0x849F, 0x1498], [0x2, 0xBC, 0x84E6, 0x1498], [0x2, 0xBC, 0x852A, 0x14A0], [0x2, 0xBC, 0x856E, 0x14A8], [0x2, 0xBC, 0x85B3, 0x14A0], [0x2, 0xBC, 0x85FA, 0x14A0], [0x2, 0xBC, 0x863E, 0x14A0], [0x2, 0xBC, 0x8682, 0x14A8], [0x2, 0x0, 0x866C, 0x0], [0x2, 0xBC, 0x870E, 0x14B8], [0x2, 0xBC, 0x8766, 0x1330], [0x2, 0xBC, 0x87AD, 0x1320], [0x2, 0xBC, 0x87F1, 0x1320], [0x2, 0x0, 0x87C7, 0x0], [0x2, 0x0, 0x880B, 0x0], [0x2, 0xBC, 0x88C1, 0x1328], [0x2, 0xBC, 0x88F1, 0x14D0], [0x2, 0xBC, 0x8936, 0x14D8], [0x2, 0xBC, 0x897D, 0x14D8], [0x2, 0xBC, 0x89C1, 0x14E8], [0x2, 0xBC, 0x8A05, 0x14F0], [0x2, 0xBC, 0x8A4C, 0x14F8], [0x2, 0xBC, 0x8A91, 0x1500], [0x2, 0xBC, 0x8AD5, 0x1500], [0x2, 0xBC, 0x8B19, 0x1508], [0x2, 0xBC, 0x8B60, 0x1510], [0x2, 0xBC, 0x8BA4, 0x1518], [0x2, 0xBC, 0x8BE9, 0x1520], [0x2, 0xBC, 0x8C30, 0x1528], [0x2, 0xBC, 0x8C74, 0x1530], [0x2, 0xBC, 0x8CB8, 0x1538], [0x2, 0xBC, 0x8D00, 0x1550], [0x2, 0x0, 0x8CE6, 0x0], [0x2, 0xBC, 0x8E88, 0x8C0], [0x2, 0xBC, 0x8ED8, 0x8B0], [0x2, 0xBC, 0x8F1C, 0x8A8], [0x2, 0xBC, 0x8F60, 0x8A8], [0x2, 0xBC, 0x8FA7, 0x8A0], [0x2, 0xBC, 0x8FEC, 0x8A0], [0x2, 0xBC, 0x9030, 0x8A8], [0x2, 0xBC, 0x9074, 0x8A8], [0x2, 0xBC, 0x90BB, 0x8B0], [0x2, 0xBC, 0x90F4, 0x8C0], [0x2, 0x0, 0x8FE0, 0x0], [0x2, 0x0, 0x9024, 0x0], [0x2, 0x0, 0x9069, 0x0], [0x2, 0x0, 0x90B0, 0x0], [0x2, 0x0, 0x90F4, 0x0], [0x2, 0x0, 0x9138, 0x0], [0x2, 0x0, 0x9180, 0x0], [0x2, 0x0, 0x91C4, 0x0], [0x2, 0x0, 0x9208, 0x0], [0x2, 0x0, 0x924C, 0x0], [0x2, 0x0, 0x9293, 0x0], [0x2, 0x0, 0x92D8, 0x0], [0x2, 0x0, 0x931C, 0x0], [0x2, 0x0, 0x9363, 0x0], [0x2, 0xBC, 0x94CF, 0x9F0], [0x2, 0xBC, 0x9513, 0x9E8], [0x2, 0xBC, 0x955B, 0x9F0], [0x2, 0xBC, 0x959F, 0xA00], [0x2, 0xBC, 0x95E3, 0xA08], [0x2, 0xBC, 0x962A, 0xA08], [0x2, 0xBC, 0x966E, 0xA08], [0x2, 0xBC, 0x96B3, 0xA10], [0x2, 0xBC, 0x96F7, 0xA18], [0x2, 0xBC, 0x9733, 0xA20], [0x2, 0xBC, 0x9777, 0xA28], [0x2, 0xBC, 0x97BE, 0xA38], [0x2, 0xBC, 0x9802, 0xA38], [0x2, 0xBC, 0x9847, 0xA48], [0x2, 0xBC, 0x988E, 0xA50]]] 88 | 89 | 90 | data = [b"\xA0\x5B\xBE\x12\x15\x11\x00\x00\x16\x01\x00\x00\x15\x01\x00\x00\x15\x01\x00\x00\x15\x01\x40\x00\x14\xD1\x7F\x01\x07\xF1\xDF\x7F\x03\xF1\xDF\x7F\x34\xD1\xBF\x00\x2A\xF1\x3F\x00\x29\x01\x00\x00\x27\x01\x00\x00\x26\x01\x00\x00\x26\x01\x00\x00\x26\x01\x00\x00\x26\x11\xC0\xFF\x29\x11\xC0\xFF\x2C\x11\x40\xFF\x32\x21\x80\xFF\x37\x11\x00\x00\x39\x01\x00\x00\x39\x11\xC0\x7F\x3B\xE1\x3E\x00\x28\x11\x80\xFF\x2C\xF1\xDF\x7F\x2F\x61\x40\xFF\x3E\x21\x40\x00\x40\xF1\xFF\xFF\x40\x01\xC0\x7F\x41\x51\xFF\x00\x31\xE1\xFF\x00\x2B\xF1\x3F\x00", b"\xAC\x58\xE7\x1B\x29\x41\xC0\xFF\x33\x21\xC0\xFF\x37\x31\x80\xFF\x3E\xF1\xDF\x7F\x40\xF1\xDF\x7F\x5A\x21\xC0\xFF\x60\x01\xC0\xFF\x62\x11\xC0\xFF\x65\x11\x40\xFF\x6B\x61\x80\xFF\x78\x11\xC0\xFE\x82\x41\x40\xFF\x8C\x21\xC0\xFF\x91\x21\x40\xFF\x99\x21\xC0\x7F\x8A\xE1\xBF\x00\x86\x31\x40\x00\x8C\xE1\x3F\x00\x89\x01\x00\x00\x89\xF1\x3F\x00\x88\x01\x00\x00\x89\x21\xC0\x7F\x8D\xF1\xDF\x7F\xAD\x31\x80\xFF\xB6\x21\x80\xFF\xBC\x11\xC0\xFF\xC1\x21\x00\x00\xC4\x01\x00\x00\xC4\xF1\x3F\x00\xC3\x11\xC0\xFF\xC6\x11\xC0\x7F\xCA\xF1\xDF\x7F", b"\xAE\x54\xF3\x24\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x54\x84\x3F\x01\x43\xB4\x7F\x01\x35\xD4\x7F\x01\x2B\xB4\x7F\x01\x1B\xD4\xBF\x00\x14\xD4\xFF\x00\x0B\xC4\x3F\x01\xFF\xE3\xFF\x00\xF7\xD3\xFF\x00\xEF\xD3\x7F\x01\xE5\xD3\xBF\x00\xE0\xC3\x7F\x00\xD8\xE3\xBF\x00\xD2\xD3\xBF\x00\xCA\xE3\x7F\x00\xC6\xE3\xBF\x00\xC0\xE3\xBF\x00\xBA\xE3\xBF\x00\xB4\xF3\x7F\x00\xB0\xF3\x7F\x00\xAD\xF3\x3F\x00\xAA\xF3\xBF\x00\xA5\xF3\x7F\x00\xA2\xF3\x7F\x00\x9F\x03\x40\x00\x9D\xF3\x7F\x00\x9A\x13\xC0\x7F\x9A\xF3\x9F\x00\x09\xF2\x3F\x00", b"\xA3\x5A\x19\x2E\x07\x02\xC0\xFF\x08\x02\xC0\xFF\x0B\xF2\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x8E\x43\x00\x00\x92\x03\x00\x00\x92\x03\xC0\xFF\x93\x03\xC0\xFF\x95\xF3\x3F\x00\x94\x13\xC0\x7F\x97\xF3\x3C\x00\x64\xF3\xDF\x7F\x65\x53\x03\x00\x9B\x23\xC0\xFF\x9F\x13\xC0\xFF\xA1\x13\xC0\xFF\xA4\x13\xC0\xFF\xA7\x33\xC0\x7F\x18\xE2\x3F\x00\x15\xF2\xFF\xFF\x15\x02\x80\xFF\x18\xF2\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x3E\xF2\xBF\xFF\x40\x12\x00\x00\x41\x12\xC0\xFF\x44\x12\x00\x00\x47\x22\xC0\xFF", b"\xAC\x53\x36\x37\x4B\x12\x80\xFF\x4F\x12\x40\xFF\x55\x02\xC0\xFF\x56\x22\xC0\xFF\x5B\x12\xC0\xFF\x5F\x22\x80\xFF\x64\x12\xC0\x7F\x68\xF2\xDF\x7F\x00\xE0\xBF\xFF\x1F\x73\x40\x01\x22\x23\xC0\x7F\x5F\x63\xC0\x7F\x6D\xF3\xDF\x7F\x00\x50\xCF\x7F\x9B\xE2\x7F\x00\x98\x02\xC0\xFF\x9A\xF2\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x00\xF0\xDF\x7F\x00\x70\xC0\x00\x60\xE3\xFF\x7F\x5B\xF3\xDF\x7F\x00\xF0\xDF\x7F\x51\x53\xFF\x7F\x46\xF3\xDF\x7F\x00\xF0\x9F\x00\x34\xF3\xDF\x7F\x30\xF3\xDF\x7F\x00\x70\x80\x00\x22\xF3\xBF\x00\x1D\xE3\x7F\x00"] 91 | 92 | capsule_prev = PyRPlidarScanUltraCapsule(data[0]) 93 | capsule_current = None 94 | 95 | for i in range(1,5): 96 | 97 | capsule_current = PyRPlidarScanUltraCapsule(data[i]) 98 | 99 | nodes = PyRPlidarScanUltraCapsule._parse_capsule(capsule_prev, capsule_current) 100 | 101 | for j in range(len(nodes)): 102 | start_flag, quality, angle_z_q14, dist_mm_q2 = nodes_result[i-1][j] 103 | self.assertEqual(start_flag, nodes[j].start_flag) 104 | self.assertEqual(quality, nodes[j].quality) 105 | self.assertEqual(angle_z_q14, nodes[j].angle_z_q14) 106 | self.assertEqual(dist_mm_q2, nodes[j].dist_mm_q2) 107 | 108 | capsule_prev = capsule_current 109 | 110 | 111 | 112 | 113 | if __name__ == "__main__": 114 | 115 | start_time = time.time() 116 | unittest.main() 117 | end_time = time.time() 118 | print("WorkingTime: {} sec".format(end_time-start_time)) 119 | --------------------------------------------------------------------------------