├── .gitignore ├── README.md ├── setup.py ├── xvlidar └── __init__.py ├── lidarplot.py └── LICENSE.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *~ 3 | __pycache__ 4 | build 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # xvlidar 2 | A simple Python class for reading from GetSurreal's XV Lidar Controller. Runs in Python2 and Python3. 3 | Run the lidarplot.py script for a demo. 4 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Python distutils setup file for installing xvlidar package 3 | 4 | Copyright (C) 2016 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | This code is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public License 16 | along with this code. If not, see . 17 | ''' 18 | 19 | from distutils.core import setup 20 | 21 | setup (name = 'xvlidar', 22 | packages = ['xvlidar'], 23 | version = '0.1', 24 | description = 'A Python class for the GetSurreal XV Lidar', 25 | author='Simon D. Levy', 26 | author_email='simon.d.levy@gmail.com', 27 | url='https://github.com/simondlevy/xvlidar', 28 | license='LGPL', 29 | platforms='Linux; Windows; OS X') 30 | -------------------------------------------------------------------------------- /xvlidar/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | xvlidar.py - Python class for reading from GetSurreal's XV Lidar Controller. 5 | 6 | Adapted from lidar.py downloaded from 7 | 8 | http://www.getsurreal.com/products/xv-lidar-controller/xv-lidar-controller-visual-test 9 | 10 | Copyright (C) 2016 Simon D. Levy 11 | 12 | This program is free software: you can redistribute it and/or modify 13 | it under the terms of the GNU Lesser General Public License as 14 | published by the Free Software Foundation, either version 3 of the 15 | License, or (at your option) any later version. 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | ''' 21 | 22 | import threading, time, serial, traceback 23 | 24 | class XVLidar(object): 25 | 26 | def __init__(self, com_port): 27 | ''' 28 | Opens a serial connection to the XV Lidar on the specifiec com port (e.g., 29 | 'COM5', '/dev/ttyACM0'). Connection will run on its own thread. 30 | ''' 31 | self.ser = serial.Serial(com_port, 115200) 32 | self.thread = threading.Thread(target=self._read_lidar, args=()) 33 | self.thread.daemon = True 34 | self.state = 0 35 | self.index = 0 36 | self.lidar_data = [()]*360 # 360 elements (distance,quality), indexed by angle 37 | self.speed_rpm = 0 38 | self.thread.start() 39 | 40 | def getScan(self): 41 | ''' 42 | Returns 360 (distance, quality) tuples. 43 | ''' 44 | return [pair if len(pair) == 2 else (0,0) for pair in self.lidar_data] 45 | 46 | def getRPM(self): 47 | ''' 48 | Returns speed in RPM. 49 | ''' 50 | return self.speed_rpm 51 | 52 | def _read_bytes(self, n): 53 | 54 | return self.ser.read(n).decode('ISO-8859-1') 55 | 56 | def _read_lidar(self): 57 | 58 | nb_errors = 0 59 | 60 | while True: 61 | 62 | try: 63 | 64 | time.sleep(0.0001) # do not hog the processor power 65 | 66 | if self.state == 0 : 67 | b = ord(self._read_bytes(1)) 68 | # start byte 69 | if b == 0xFA : 70 | self.state = 1 71 | else: 72 | self.state = 0 73 | elif self.state == 1: 74 | # position index 75 | b = ord(self._read_bytes(1)) 76 | if b >= 0xA0 and b <= 0xF9 : 77 | self.index = b - 0xA0 78 | self.state = 2 79 | elif b != 0xFA: 80 | self.state = 0 81 | elif self.state == 2 : 82 | 83 | data = [ord(b) for b in self._read_bytes(20)] 84 | 85 | # speed 86 | b_speed = data[:2] 87 | 88 | # data 89 | b_data0 = data[2:6] 90 | b_data1 = data[6:10] 91 | b_data2 = data[10:14] 92 | b_data3 = data[14:18] 93 | 94 | # checksum 95 | b_checksum = data[18:20] 96 | 97 | # for the checksum, we need all the data of the packet... 98 | # this could be collected in a more elegent fashion... 99 | all_data = [ 0xFA, self.index+0xA0 ] + data[:18] 100 | 101 | # checksum 102 | incoming_checksum = int(b_checksum[0]) + (int(b_checksum[1]) << 8) 103 | 104 | # verify that the received checksum is equal to the one computed from the data 105 | if self._checksum(all_data) == incoming_checksum: 106 | 107 | self.speed_rpm = float( b_speed[0] | (b_speed[1] << 8) ) / 64.0 108 | 109 | self._update(0, b_data0) 110 | self._update(1, b_data1) 111 | self._update(2, b_data2) 112 | self._update(3, b_data3) 113 | 114 | else: 115 | # the checksum does not match, something went wrong... 116 | nb_errors +=1 117 | print('Checksum fail') 118 | 119 | 120 | self.state = 0 # reset and wait for the next packet 121 | 122 | else: # default, should never happen... 123 | self.state = 0 124 | 125 | except: 126 | traceback.print_exc() 127 | exit(0) 128 | 129 | def _update(self, offset, data ): 130 | 131 | angle = self.index * 4 + offset 132 | 133 | #unpack data using the denomination used during the discussions 134 | x = data[0] 135 | x1= data[1] 136 | x2= data[2] 137 | x3= data[3] 138 | 139 | dist_mm = x | (( x1 & 0x3f) << 8) # distance is coded on 13 bits ? 14 bits ? 140 | quality = x2 | (x3 << 8) # quality is on 16 bits 141 | self.lidar_data[angle] = dist_mm,quality 142 | 143 | def _checksum(self, data): 144 | """Compute and return the checksum as an int. 145 | data -- list of 20 bytes (as ints), in the order they arrived in. 146 | """ 147 | # group the data by word, little-endian 148 | data_list = [] 149 | for t in range(10): 150 | data_list.append( data[2*t] + (data[2*t+1]<<8) ) 151 | 152 | # compute the checksum on 32 bits 153 | chk32 = 0 154 | for d in data_list: 155 | chk32 = (chk32 << 1) + d 156 | 157 | # return a value wrapped around on 15bits, and truncated to still fit into 15 bits 158 | checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits 159 | checksum = checksum & 0x7FFF # truncate to 15 bits 160 | return int( checksum ) 161 | -------------------------------------------------------------------------------- /lidarplot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | xvlidarplot.py : A little Python class to display scans from the XV Lidar 5 | 6 | Copyright (C) 2016 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | #COM_PORT = 'COM42' # Windows 23 | COM_PORT = '/dev/ttyACM0' # Linux 24 | 25 | # Arbitrary display params 26 | DISPLAY_CANVAS_SIZE_PIXELS = 500 27 | DISPLAY_CANVAS_COLOR = 'black' 28 | DISPLAY_SCAN_LINE_COLOR = 'yellow' 29 | 30 | # XVLIDAR-04LX specs 31 | XVLIDAR_MAX_SCAN_DIST_MM = 6000 32 | XVLIDAR_DETECTION_DEG = 360 33 | XVLIDAR_SCAN_SIZE = 360 34 | 35 | from xvlidar import XVLidar 36 | 37 | from math import sin, cos, radians 38 | from time import time, sleep, ctime 39 | from sys import exit, version 40 | 41 | if version[0] == '3': 42 | import tkinter as tk 43 | import _thread as thread 44 | else: 45 | import Tkinter as tk 46 | import thread 47 | 48 | 49 | 50 | class XVLidarPlotter(tk.Frame): 51 | ''' 52 | XVLidarPlotter extends tk.Frame to plot Lidar scans. 53 | ''' 54 | 55 | def __init__(self): 56 | ''' 57 | Takes no args. Maybe we could specify colors, lidar params, etc. 58 | ''' 59 | 60 | # Create the frame 61 | tk.Frame.__init__(self, borderwidth = 4, relief = 'sunken') 62 | self.master.geometry(str(DISPLAY_CANVAS_SIZE_PIXELS)+ "x" + str(DISPLAY_CANVAS_SIZE_PIXELS)) 63 | self.master.title('XV Lidar [ESC to quit]') 64 | self.grid() 65 | self.master.rowconfigure(0, weight = 1) 66 | self.master.columnconfigure(0, weight = 1) 67 | self.grid(sticky = tk.W+tk.E+tk.N+tk.S) 68 | self.background = DISPLAY_CANVAS_COLOR 69 | 70 | # Add a canvas for drawing 71 | self.canvas = tk.Canvas(self, \ 72 | width = DISPLAY_CANVAS_SIZE_PIXELS, \ 73 | height = DISPLAY_CANVAS_SIZE_PIXELS,\ 74 | background = DISPLAY_CANVAS_COLOR) 75 | self.canvas.grid(row = 0, column = 0,\ 76 | rowspan = 1, columnspan = 1,\ 77 | sticky = tk.W+tk.E+tk.N+tk.S) 78 | 79 | # Set up a key event for exit on ESC 80 | self.bind('', self._key) 81 | 82 | # This call gives the frame focus so that it receives input 83 | self.focus_set() 84 | 85 | # No scanlines initially 86 | self.lines = [] 87 | 88 | # Connect to the XVLidar 89 | self.lidar = XVLidar(COM_PORT) 90 | 91 | # No scan data to start 92 | self.scandata = [] 93 | 94 | # Pre-compute some values useful for plotting 95 | 96 | scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \ 97 | XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)] 98 | 99 | self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2 100 | scale = self.half_canvas_pix / float(XVLIDAR_MAX_SCAN_DIST_MM) 101 | 102 | self.cos = [-cos(angle) * scale for angle in scan_angle_rad] 103 | self.sin = [ sin(angle) * scale for angle in scan_angle_rad] 104 | 105 | # Add scan lines to canvas, to be modified later 106 | self.lines = [self.canvas.create_line(\ 107 | self.half_canvas_pix, \ 108 | self.half_canvas_pix, \ 109 | self.half_canvas_pix + self.sin[k] * XVLIDAR_MAX_SCAN_DIST_MM,\ 110 | self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM) 111 | for k in range(XVLIDAR_SCAN_SIZE)] 112 | 113 | [self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR) for line in self.lines] 114 | 115 | # Start a new thread and set a flag to let it know when we stop running 116 | thread.start_new_thread( self.grab_scan, () ) 117 | self.running = True 118 | 119 | # Runs on its own thread 120 | def grab_scan(self): 121 | 122 | while True: 123 | 124 | # Lidar sends 360 (distance, quality) pairs, which may be empty on start 125 | self.scandata = [pair[0] for pair in self.lidar.getScan()] 126 | 127 | self.count += 1 128 | 129 | if not self.running: 130 | break 131 | 132 | # yield to plotting thread 133 | sleep(.0001) 134 | 135 | def run(self): 136 | ''' 137 | Call this when you're ready to run. 138 | ''' 139 | 140 | # Record start time and initiate a count of scans for testing 141 | self.count = 0 142 | self.start_sec = time() 143 | self.showcount = 0 144 | 145 | # Start the recursive timer-task 146 | plotter._task() 147 | 148 | # Start the GUI 149 | plotter.mainloop() 150 | 151 | 152 | def destroy(self): 153 | ''' 154 | Called automagically when user clicks X to close window. 155 | ''' 156 | 157 | self._quit() 158 | 159 | def _quit(self): 160 | 161 | self.running = False 162 | elapsed_sec = time() - self.start_sec 163 | 164 | del self.lidar 165 | 166 | exit(0) 167 | 168 | def _key(self, event): 169 | 170 | # Make sure the frame is receiving input! 171 | self.focus_force() 172 | if event.keysym == 'Escape': 173 | self._quit() 174 | 175 | def _task(self): 176 | 177 | # Modify the displayed lines according to the current scan 178 | [self.canvas.coords(self.lines[k], 179 | self.half_canvas_pix, \ 180 | self.half_canvas_pix, \ 181 | self.half_canvas_pix + self.sin[k] * self.scandata[k],\ 182 | self.half_canvas_pix + self.cos[k] * self.scandata[k]) \ 183 | for k in range(len(self.scandata))] 184 | 185 | # Reschedule this task immediately 186 | self.after(1, self._task) 187 | 188 | # Record another display for reporting performance 189 | self.showcount += 1 190 | 191 | 192 | # Instantiate and pop up the window 193 | if __name__ == '__main__': 194 | 195 | plotter = XVLidarPlotter() 196 | 197 | plotter.run() 198 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | --------------------------------------------------------------------------------