def main():
lidar = Lidar(device='/dev/ttyACM0')
slam = RMHC_SLAM(LaserModel(), map_size_pixels=MAP_SIZE_PIXELS, map_size_meters=MAP_SIZE_METERS)
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
#image_thread = threading.Thread(target=save_image, args=[display])
#image_thread.start()
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
slam.update(lidar.getScan())
x, y, theta = slam.getpos()
#print(x, y, theta)
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.setPose(x, y, theta)
#display.save_image()
display.save_pgm(mapbytes)
if not display.refresh():
exit(0)
def __init__(self, readFrom):
'''
Takes no args. Maybe we could specify colors, lidar params, etc.
'''
self.bytesFromLIDAR = []
# No scan data to start
self.scandata = []
self.scanCount = 0
self.running = True
self.readFrom = readFrom
if readFrom == READ_FROM_FILE:
##read from log file
self.file = open('urg04-LX-log', 'rb')
self.bytesFromLIDAR = self.file.read()
self.scanIndex = 0
elif readFrom == READ_FROM_SERIAL:
# Create a URG04LX object and connect to it
self.lidar = URG04LX(URG_DEVICE)
#thread.start_new_thread( grab_scan, (self,) )
else:
self.lidar = URG04LX(URG_DEVICE)
self.readScanFromLidarAndWriteToFile()
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('Hokuyo URG04LX [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 = []
# Create a URG04LX object and connect to it
self.lidar = URG04LX(URG_DEVICE)
# No scan data to start
self.scandata = []
# Pre-compute some values useful for plotting
scan_angle_rad = [radians(-URG_DETECTION_DEG/2 + (float(k)/URG_SCAN_SIZE) * \
URG_DETECTION_DEG) for k in range(URG_SCAN_SIZE)]
self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
scale = self.half_canvas_pix / float(URG_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] * URG_MAX_SCAN_DIST_MM,\
self.half_canvas_pix + self.cos[k] * URG_MAX_SCAN_DIST_MM)
for k in range(URG_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(grab_scan, (self, ))
self.running = True
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
LIDAR_DEVICE = '/dev/ttyACM0'
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import URG04LX as LaserModel
from breezylidar import URG04LX as Lidar
from pltslamshow import SlamShow
if __name__ == '__main__':
# Connect to Lidar unit
lidar = Lidar(LIDAR_DEVICE)
# 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 empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan
slam.update(lidar.getScan())
from time import time
lidarMin = 114
lidarMax = 568
nearMin = 158
nearMax = 524
farMin = 318
farMax = 364
stepDeg = 682 / 240
degStep = 240 / 682
lidarPort = '/dev/ttyACM0'
laser = URG04LX(lidarPort)
failCount = 0
print('Started Lidar\n===================')
data = []
for i in range(0, 682):
data.append(0)
for i in range(0, 5):
scan = laser.getScan()
for j in range(0, len(scan)):
data[j] += scan[j]
for j in range(0, len(scan)):
data[j] /= 5
farSum = 0
def __init__(self):
laser = URG04LX(DEVICE)
self.deq = collections.deque(maxlen=1)
_thread.start_new_thread(producer, (self.deq, laser))
def main():
laser = URG04LX('/dev/ttyACM0')
while True:
scan = laser.getScan()
print(scan)
#!/usr/bin/env python3
from breezylidar import URG04LX
import sys
import json
from time import time
f = open('log.json', 'a+')
DEVICE = '/dev/ttyACM0'
NITER = 10
laser = URG04LX(DEVICE)
print('===============================================================')
print('Detected lidar')
print(laser)
print('===============================================================')
while True:
start_sec = time()
count = 0
for i in range(1, NITER + 1):
sys.stdout.write('Iteration: %3d: ' % i)
data = laser.getScan()
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
LIDAR_DEVICE = '/dev/ttyACM0'
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import URG04LX as LaserModel
from breezylidar import URG04LX as Lidar
from roboviz import MapVisualizer
if __name__ == '__main__':
# Connect to Lidar unit
lidar = Lidar(LIDAR_DEVICE)
# 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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan
slam.update(lidar.getScan())