#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
