# Create an RMHC SLAM object with a laser model and optional robot model slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') # Initialize an empty trajectory trajectory = [] # Initialize empty map mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) while True: # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs slam.update([pair[0] for pair in lidar.getScan()]) # Get current robot position x, y, theta = slam.getpos() # Get current map bytes as grayscale slam.getmap(mapbytes) # Display map display.displayMap(mapbytes) # Display pose after converting from mm to meters display.setPose(x / 1000., y / 1000., theta) # Exit on window close if not display.refresh():
class XVLidarPlotter(tk.Frame): ''' XVLidarPlotter 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 = XVLidar(COM_PORT) # No scan data to start self.scandata = [] # Pre-compute some values useful for plotting scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \ XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)] self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2 scale = self.half_canvas_pix / float(XVLIDAR_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] * XVLIDAR_MAX_SCAN_DIST_MM,\ self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM) for k in range(XVLIDAR_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
# Create an RMHC SLAM object with a laser model and optional robot model slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM') # Initialize an empty trajectory trajectory = [] # Initialize empty map mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) while True: lidar_scan = lidar.getScan() telem = robot.get_telem() vel = odomRobot.computeVelocities(telem[0], telem[1], telem[2]) # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs slam.update([pair[0] for pair in lidar_scan], vel) # Get current robot position x, y, theta = slam.getpos() print '(' + str(x) + ',' + str(y) + ',' + str(theta) + ')' # Get current map bytes as grayscale slam.getmap(mapbytes) display.displayMap(mapbytes)
trajectory = [] # Initialize empty map mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) #wait for positioning to be ready ready = "0" while ready = "0" file_isup = open("/var/lib/mercator/isup","r"); ready = file_isup.read(1); file_isup.close(); while True: # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs slam.update([pair[0] for pair in lidar.getScan()]) # Get current robot position x, y, theta = slam.getpos() # Get current map bytes as grayscale slam.getmap(mapbytes) display.displayMap(mapbytes) display.displayRobot((x, y, theta)) trajectory.append((x,y)) # Display trajectory display.displayTrajectory(trajectory)
class XVLidarPlotter(tk.Frame): ''' XVLidarPlotter 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 = XVLidar(COM_PORT) # No scan data to start self.scandata = [] # Pre-compute some values useful for plotting scan_angle_rad = [radians(-XVLIDAR_DETECTION_DEG/2 + (float(k)/XVLIDAR_SCAN_SIZE) * \ XVLIDAR_DETECTION_DEG) for k in range(XVLIDAR_SCAN_SIZE)] self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2 scale = self.half_canvas_pix / float(XVLIDAR_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] * XVLIDAR_MAX_SCAN_DIST_MM,\ self.half_canvas_pix + self.cos[k] * XVLIDAR_MAX_SCAN_DIST_MM) for k in range(XVLIDAR_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