#GLOBALS done = 0 def signal_handler(signal, frame): global done done = 1 if version[0] == '3': import _thread as thread else: import thread if __name__ == '__main__': signal.signal(signal.SIGINT, signal_handler) lidar = RPLidar(COM_PORT) while done == 0 : time.sleep(1) distances = [pair[0] for pair in lidar.getScan()] print " New Scan : angle | distance" i = 0 while i < 360: print " {angle} | {distance}".format(angle = i, distance = distances[i]) i = i+1 lidar.set_exitflag() print"" print "DONE"
# Initialize an empty trajectory trajectory = [] # Initialize empty map mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) time.sleep(120) print "Ready to drive.\n" # Initialize flags & cooldown timer for saving map start_time = time.clock() save_flag = 0 while done == 0: time.sleep(1) #Do not hog processing power # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs scan = [pair[0] for pair in lidar.getScan()] print "distance 0 = {dist}".format(dist=scan[0]) slam.update(scan) # Get current robot position x, y, theta = slam.getpos() trajectory.append((x,y)) current_time = time.clock() print "Executed.. angle = {theta}".format(theta = theta) print "\n\n" if (int(current_time - start_time)%30) == 3 : if save_flag == 1: save_flag = 0 # Get current map bytes as grayscale
done = 0 def signal_handler(signal, frame): global done done = 1 if version[0] == '3': import _thread as thread else: import thread if __name__ == '__main__': signal.signal(signal.SIGINT, signal_handler) lidar = RPLidar(COM_PORT) while done == 0: time.sleep(1) distances = [pair[0] for pair in lidar.getScan()] print " New Scan : angle | distance" i = 0 while i < 360: print " {angle} | {distance}".format( angle=i, distance=distances[i]) i = i + 1 lidar.set_exitflag() print "" print "DONE"
class RPLidarPlotter(tk.Frame): ''' RPLidarPlotter extends tk.Frame to plot Lidar scans. ''' def __init__(self): ''' Takes no args. Maybe we could specify colors, lidar params, etc. ''' # Create the frame tk.Frame.__init__(self, borderwidth = 4, relief = 'sunken') self.master.geometry(str(DISPLAY_CANVAS_SIZE_PIXELS)+ "x" + str(DISPLAY_CANVAS_SIZE_PIXELS)) self.master.title('XV Lidar [ESC to quit]') self.grid() self.master.rowconfigure(0, weight = 1) self.master.columnconfigure(0, weight = 1) self.grid(sticky = tk.W+tk.E+tk.N+tk.S) self.background = DISPLAY_CANVAS_COLOR # Add a canvas for drawing self.canvas = tk.Canvas(self, \ width = DISPLAY_CANVAS_SIZE_PIXELS, \ height = DISPLAY_CANVAS_SIZE_PIXELS,\ background = DISPLAY_CANVAS_COLOR) self.canvas.grid(row = 0, column = 0,\ rowspan = 1, columnspan = 1,\ sticky = tk.W+tk.E+tk.N+tk.S) # Set up a key event for exit on ESC self.bind('<Key>', self._key) # This call gives the frame focus so that it receives input self.focus_set() # No scanlines initially self.lines = [] # Connect to the XVLidar self.lidar = RPLidar(COM_PORT) # No scan data to start self.scandata = [] # Pre-compute some values useful for plotting scan_angle_rad = [radians(-RPLIDAR_DETECTION_DEG/2 + (float(k)/RPLIDAR_SCAN_SIZE) * \ RPLIDAR_DETECTION_DEG) for k in range(RPLIDAR_SCAN_SIZE)] self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2 scale = self.half_canvas_pix / float(RPLIDAR_MAX_SCAN_DIST_MM) self.cos = [-cos(angle) * scale for angle in scan_angle_rad] self.sin = [ sin(angle) * scale for angle in scan_angle_rad] # Add scan lines to canvas, to be modified later self.lines = [self.canvas.create_line(\ self.half_canvas_pix, \ self.half_canvas_pix, \ self.half_canvas_pix + self.sin[k] * RPLIDAR_MAX_SCAN_DIST_MM,\ self.half_canvas_pix + self.cos[k] * RPLIDAR_MAX_SCAN_DIST_MM) for k in range(RPLIDAR_SCAN_SIZE)] [self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR) for line in self.lines] # Start a new thread and set a flag to let it know when we stop running thread.start_new_thread( self.grab_scan, () ) self.running = True # Runs on its own thread def grab_scan(self): while True: # Lidar sends 360 (distance, quality) pairs, which may be empty on start self.scandata = [pair[0] for pair in self.lidar.getScan()] self.count += 1 if not self.running: break # yield to plotting thread sleep(.0001) def run(self): ''' Call this when you're ready to run. ''' # Record start time and initiate a count of scans for testing self.count = 0 self.start_sec = time() self.showcount = 0 # Start the recursive timer-task plotter._task() # Start the GUI plotter.mainloop() def destroy(self): ''' Called automagically when user clicks X to close window. ''' self._quit() def _quit(self): self.running = False elapsed_sec = time() - self.start_sec del self.lidar exit(0) def _key(self, event): # Make sure the frame is receiving input! self.focus_force() if event.keysym == 'Escape': self._quit() def _task(self): # Modify the displayed lines according to the current scan [self.canvas.coords(self.lines[k], self.half_canvas_pix, \ self.half_canvas_pix, \ self.half_canvas_pix + self.sin[k] * self.scandata[k],\ self.half_canvas_pix + self.cos[k] * self.scandata[k]) \ for k in range(len(self.scandata))] # Reschedule this task immediately self.after(1, self._task) # Record another display for reporting performance self.showcount += 1