def run():
lidar = RPLidar(PORT_NAME)
time.sleep(WAIT)
status, code = lidar.get_health()
time.sleep(WAIT)
lidar.disconnect()
print(status)
def __init__(self, wheel_radius, wheel_dist, ARDUINO_HCR, LIDAR_DEVICE):
# Connect to Arduino unit
self.arduino = Serial(ARDUINO_HCR, 57600)
# Connect to Lidar unit
self.lidar = RPLidar(LIDAR_DEVICE)
# Create an iterator to collect scan data from the RPLidar
time.sleep(1)
self.iterator = self.lidar.iter_scans()
# First scan is crap, so ignore it
#next(self.iterator)
self.WHEELS_DIST = wheel_dist
self.WHEELS_RAD = wheel_radius
self.x = 0
self.y = 0
self.yaw = 0
self.linear_velocity = 0
self.angular_velocity = 0
self.omegaRight = 0
self.omegaLeft = 0
self.vr = 0
self.vl = 0
self.scan = []
self.prev_time = time.time()
self.current_time = time.time()
self.dt = 0
def stop(port):
lidar = RPLidar(port)
lidar.start_motor()
sleep(0.01)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def __init__(self,
device=RPLIDAR_DEVICE,
topic_prefix=MQTT_TOPIC_PREFIX,
host=MQTT_BROKER_HOST,
port=MQTT_BROKER_PORT,
reset=False):
self.mqtt = mqtt.Client(client_id="rplidar_mqtt_bridge",
clean_session=True)
self.mqtt.on_connect = on_mqtt_connect
self.mqtt.connect(host=host, port=port)
self.lidar = RPLidar(device)
# Generate a unique topic identifier by using the MD5 hash of the device serial number
info = self.lidar.get_info()
serial = info['serialnumber']
serial_hash = hashlib.md5(serial.encode()).hexdigest()
self.topic_prefix = topic_prefix + '/' + serial_hash
if reset is True:
self.clear_rplidar_readings()
self.report_rplidar_source()
self.report_rplidar_info(info)
def connect():
global lidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
lidar.clear_input()
health = lidar.get_health()
print(health)
def __init__(self, lower_limit=0, upper_limit=360, debug=False):
from rplidar import RPLidar
import glob
port_found = False
self.lower_limit = lower_limit
self.upper_limit = upper_limit
temp_list = glob.glob('/dev/ttyUSB*')
result = []
for a_port in temp_list:
try:
s = serial.Serial(a_port)
s.close()
result.append(a_port)
port_found = True
except serial.SerialException:
pass
if port_found:
self.port = result[0]
self.distances = [] #a list of distance measurements
self.angles = [
] # a list of angles corresponding to dist meas above
self.lidar = RPLidar(self.port, baudrate=115200)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
else:
print("No Lidar found")
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.frame = np.zeros(shape=365)
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
def __init__(self, cfg):
super().__init__(inputs=[], outputs=['lidar', ], threaded=True)
self.port = cfg['lidar']
self.distance = [] # List distance, will be placed in datastorage
self.angles = [] # List angles, will be placed in datastorage
self.lidar = RPLidar(self.port)
self.on = True
self.lidar.clear_input()
def __init__(self):
# Initialize Lidar Lidar
self.lidar = RPLidar('com3')
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
def __init__(self, on_map_change):
super().__init__(on_map_change=on_map_change)
self.lidar = RPLidar(os.getenv('LIDAR_DEVICE', SLAM["LIDAR_DEVICE"]))
self.slam = RMHC_SLAM(RPLidarA1(), SLAM['MAP_SIZE_PIXELS'], SLAM['MAP_SIZE_METERS'])
self.map_size_pixels = SLAM['MAP_SIZE_PIXELS']
self.trajectory = []
self.mapbytes = bytearray(SLAM['MAP_SIZE_PIXELS'] * SLAM['MAP_SIZE_PIXELS'])
self.prev_checksum = 0
def connect(addr):
lidar = RPLidar(addr)
info = lidar.get_info()
#print(info)
health = lidar.get_health()
#print(health)
return lidar
def lidar_stop(self):
if self.process.poll() is None:
self.process.kill()
while self.process.poll() is None:
pass
self.lidar = RPLidar(self.lidar_port)
self.lidar.connect()
self.lidar.stop_motor()
self.lidar.stop()
def connect():
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
return lidar
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.distances = [] #a list of distance measurements
self.angles = [] # a list of angles corresponding to dist meas above
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
def __init__(self, port):
threading.Thread.__init__(self)
self.lidar = RPLidar(port)
self.lidar.stop_motor()
sleep(0.5)
self.lidar.start_motor()
self.setDaemon(True)
self.scan = [(0.0, 0.0) for i in range(360)]
def __init__(self, port):
self.port = port
self.lidar = RPLidar('/dev/tty.SLAB_USBtoUART')
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
def __init__(self, config, mbus, event, lidar_data):
threading.Thread.__init__(self)
self.config = config
self.message_bus = mbus
self.event = event
self.lidar_data = lidar_data
self._running = True
self.lidar_controller = RPLidar(config['lidar_port_name'])
self.lidar_controller.stop()
self.lidar_updates_topic_name = TopicNames.lidar
def scan(self):
pointCloud = []
lidar = RPLidar(self.portname)
for i, measurement in enumerate(lidar.iter_measures()):
for j, v in enumerate(measurement):
if j == 0:
newscan = v
if j == 1:
quality = v
if j == 2:
angle = v
if j == 3:
length = v
# change angle to match orientation of vehicle
angle = -1 * (angle - 90)
pointCloud.append([angle, length])
if i > 360:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return pointCloud
def main():
lidar = RPLidar(config.LIDAR_PORT_NAME)
time.sleep(5)
measurments_list = []
obstacles = {"left": False, "center": False, "right": False}
for measurment in lidar.iter_measurments(max_buf_meas=config.MAX_BUF_MEAS):
measurments_list.append(measurment)
if len(measurments_list) >= config.NUMBER_MEASURE:
_, frame = cap.read()
frame = cv2.resize(frame, (widthScreen, heightScreen)) #image
obstacles = find_obstacles(measurments_list)
results = trackedObjectDirection(frame, obstacles)
commandsTable[convert(results)][convert(obstacles)]()
measurments_list.clear()
cv2.imshow("camera", frame)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
###release the capture###
#cap.release()
#Close output window
cv2.destroyAllWindows()
#Stop RPLidar
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
#send signal to stop motor to Arduino
### time.sleep() di config.FREQ_COMMANDS in modo da evitare che non sia inviato###
#time.sleep(config.FREQ_COMMANDS)
stops()
def runLidar_client(self):
""" Spins the lidar, parses and stores the readings.
If jetson sends a list of bounding boxes that are detected,
the program temporarily stops the lidar from scanning so that
it can access the cache
*** We use a condition_variable to make this main thread sleep
until we find the depth in our |lidar_readings| dictionary
"""
lidar = RPLidar('/dev/ttyUSB0')
startTime = time.time()
stopMessage = [False]
stopReceiever = Thread(target=self.stopLidarThread,
args=(stopMessage, ))
stopReceiever.start()
f = open("scan_reading.txt", "w+")
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=5000)):
f.write("%s\n" % (scan))
while not self._jetsonMsgCv.isSet():
self._jetsonMsgCv.wait()
if stopMessage[0] or i == c.MAX_NUM_ROTATION:
break
# unixTimeStamp made here. Round to the nearest second
currentTime = round(time.time())
self.parseLidarData(scan, currentTime)
lidar.stop()
f.close()
lidar.stop_motor()
lidar.disconnect()
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
cnt = 0
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
outfile.write(line + '\n')
cnt += 1
if lidar.motor:
ln = 'Spinning %d'
else:
ln = 'Stopped %d'
print(ln % cnt)
if cnt > 360:
break
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
def run():
"""Create a live plot of the data points."""
lidar = RPLidar('/dev/tty.SLAB_USBtoUART')
time.sleep(0.1)
fig, ax = plt.subplots(figsize=(8, 8))
ax.axis('scaled')
ax.axis([-10, 10, -10, 10])
ax.grid(True)
iterator = lidar.iter_scans()
points = ax.scatter([0, 0], [0, 0], s=5, lw=0)
line, = ax.plot([0, 1], [0, 1])
ani = animation.FuncAnimation(fig,
update_line,
fargs=(iterator, points, line),
interval=50)
plt.show()
lidar.stop()
lidar.stop_motor()
lidar._serial_port.reset_input_buffer()
lidar._serial_port.reset_output_buffer()
lidar.disconnect()
def lidar_logger(timestamp):
global INLOCSYNC, DONE, LIDAR_STATUS
port_name = '/dev/ttyUSB0'
while not INLOCSYNC:
time.sleep(5)
while not DONE:
try:
lidar = RPLidar(port_name)
with open("/root/gps-data/%s_lidar.csv" % timestamp, "w") as f:
f.write("%s\n" % VERSION)
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=1000)):
t = time.time()
lidartime = datetime.datetime.now().strftime(
"%Y-%m-%dT%H:%M:%S.%fZ")
f.write("%s L [" % (lidartime))
for (_, angle, distance) in scan:
f.write("(%0.4f,%0.2f)," % (angle, distance))
f.write("] *\n")
LIDAR_STATUS = True
if DONE:
break
LIDAR_STATUS = False
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
except KeyboardInterrupt:
DONE = True
except Exception as ex:
print("WARNING: %s" % ex)
time.sleep(5)
timestamp = time.strftime("%Y%m%d%H%M", time.gmtime(time.time()))
print("LIDAR Done")
def update_line(num, iterator, ax, Aport, lidar):
print(num)
try:
scan = next(iterator)
except:
print("Second attempt for a Lidar Scan")
lidar = RPLidar(PORT_NAME) # Connect to the RPLidar Port
iterator = lidar.iter_scans() # Object to pull scans from the RPLidar
scan = next(iterator)
"""
Iterator returns 3 arguments:
meas(0) -> quality : int
Reflected laser pulse strength
Meas(1) -> angle : float
The measurement heading angle in degree unit [0, 360)
meas(2) -> distance : float
Measured object distance related to the sensor's rotation center.
In millimeter unit. Set to 0 when measurement is invalid.
"""
# Pass measurements to related variables
try:
# First Attempt to read an angle value from the MCU Serial Port
line = Aport.readline()
#Aport.write(b'value received')
line = str(line, "utf-8")
angle = int(re.sub("[^0-9]", "", line))
# If a misread occurs, try again
except:
# Second attempt to read an angle value from the MCU Serial Port
# print("Angle misread, second attempt")
line = Aport.readline()
# Aport.write(b'value received')
line = str(line, "utf-8")
angle = int(re.sub("[^0-9]", "", line))
# Affine Transform:
# angle = (Input-Inputstart)/(inputend - inputstart)*(outputend- outputstart)
angle = (angle) * (360 - 0) / (1024 - 0) + 0
theta = np.array([(np.radians(meas[1])) for meas in scan])
R = np.array([meas[2] for meas in scan])
phi = np.deg2rad(
angle
) # Test value in the phi angle, to be switched with Arduino object value
# Perform Spherical to Cartesian Conversion
X = R * np.sin(phi) * np.cos(theta)
Y = R * np.sin(phi) * np.sin(theta)
Z = R * np.cos(phi)
offsets = np.array([X, Y, Z])
ax.clear()
ax.set_zlim(-2500, 2500)
ax.set_xlim(-2500, 2500)
ax.set_ylim(-2500, 2500)
ax.scatter3D(X, Y, Z)
return X, Y, Z
def run():
lidar = RPLidar(PORT_NAME)
fig = plt.figure() #
ax = plt.axes(projection = 'polar')
line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
cmap=plt.cm.Greys_r, lw=0)
ax.set_rmax(DMAX)
ax.grid(True)
iterator = lidar.iter_scans()
ani = animation.FuncAnimation(fig, update_line,
fargs=(iterator, line), interval=50)
"""
animation.FuncAnimation arguments:
class matplotlib.animation.FuncAnimation(fig, func, frames=None, init_func=None, fargs=None, save_count=None, **kwargs)
fig -> figure object
func -> The function to call at each frame. The first argument will be the next value in frames.
Any additional positional arguments can be supplied via the fargs parameter.
fargs -> Additional arguments to pass to each call to func.
interval -> Delay between frames in milliseconds. Defaults to 200.
"""
plt.show()
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run():
lidar = RPLidar(config['lidar']['port'])
pygame.init()
points = [(0, 0) for i in range(361)]
k = 0
try:
print('Initializing')
time.sleep(5)
print('Recording data')
for new, quality, theta, r in lidar.iter_measurments(max_buf_meas=800):
x = (math.cos(math.radians(theta / math.pi) * math.pi) * r) / int(
config['scale'])
y = (math.sin(math.radians(theta / math.pi) * math.pi) * r) / int(
config['scale'])
points[int(theta * 1)] = (x, y)
k += 1
if k > 100:
k = 0
draw(points)
except KeyboardInterrupt:
print('Stopping')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def _measured_distance(self):
"""."""
try:
lidar = RPLidar(self.PORT_NAME)
for measurment in lidar.iter_measurments():
line = '\n'.join(str(v) for v in measurment)
newline = line.split("\n")
if ((float(newline[2]) > 0 and 0.3 > float(newline[2])) or
(float(newline[2]) > 359.7 and 360 > float(newline[2]))):
distance = float(newline[3]) / 10
lidar.stop()
lidar.disconnect()
time.sleep(0.2)
break
self.log.log("_measured_distance() successful",
level=3,
days_to_remain=1)
return (distance)
except:
self.log.log("_measured_distance() exception",
level=3,
days_to_remain=1)
lidar.disconnect()
time.sleep(0.2)
self._measured_distance()
class Lidar(QThread):
# Signal pour envoyer les scans
newScansReceived = pyqtSignal(np.ndarray, np.ndarray, np.ndarray,
np.ndarray)
def __init__(self):
super().__init__()
self.lidar = RPLidar('COM12', baudrate=256000)
#self.lidar = RPLidar('/dev/ttyUSB0', baudrate=256000)
#self.connect()
print('connexion avec le lidar')
self.lidar.logger.setLevel(logging.DEBUG)
consoleHandler = logging.StreamHandler()
#self.lidar.logger.addHandler(consoleHandler)
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
print("####################")
time.sleep(2)
self.continuer = True
def run(self):
gen = self.lidar.scan2ArrayFromLidar(45, 135, 225, 315)
while self.continuer:
radiusRight, thetaRight, radiusLeft, thetaLeft = next(gen)
self.newScansReceived.emit(radiusRight, thetaRight, radiusLeft,
thetaLeft)
print("fin du thread Lidar")
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
class Scanner(threading.Thread):
def __init__(self, port):
threading.Thread.__init__(self)
self.lidar = RPLidar(port)
self.lidar.stop_motor()
sleep(0.5)
self.lidar.start_motor()
self.setDaemon(True)
self.scan = [(0.0, 0.0) for i in range(360)]
def check_device(self):
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
return (info, health)
def get_scan(self):
return self.scan
def run(self):
self.lidar.clear_input()
while True:
for meas in self.lidar.iter_measurments():
new_scan = meas[0]
quality = meas[1]
angle = meas[2]
distance = meas[3]
if angle < 360:
self.scan[int(angle)] = (angle, distance / 1000.0)
def updateObstacles(obstacleBooleans):
lidar = RPLidar('/dev/ttyUSB0')
time.sleep(0.1)
try:
# for new_scan, quality, angle, distance in lidar.iter_measurments():
# if angle < 0.5 or angle > 359.5:
# print(quality, distance)
# obstacleBooleans.value = distance < 800
for scan in lidar.iter_scans():
distances = [x[2]/10 for x in filter(
lambda x: 170 < x[1] and x[1] < 190, scan)]
N = len(distances)
if N:
mean = sum(distances) / N
print(mean)
obstacleBooleans.value = mean < 80
else:
obstacleBooleans.value = False
except KeyboardInterrupt:
print('Keyboard interrupt, stopping and disconnecting lidar.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
data = []
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
data.append(np.array(scan))
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
np.save(path, np.array(data))
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
outfile.write(line + '\n')
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
outfile.close()
def run():
lidar = RPLidar(PORT_NAME)
fig = plt.figure()
ax = plt.subplot(111, projection='polar')
line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
cmap=plt.cm.Greys_r, lw=0)
ax.set_rmax(DMAX)
ax.grid(True)
iterator = lidar.iter_scans()
ani = animation.FuncAnimation(fig, update_line,
fargs=(iterator, line), interval=50)
plt.show()
lidar.stop()
lidar.disconnect()
def run():
'''Main function'''
plt.ion()
lidar = RPLidar(PORT_NAME)
subplot = plt.subplot(111, projection='polar')
plot = subplot.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
cmap=plt.cm.Greys_r, lw=0)
subplot.set_rmax(DMAX)
subplot.grid(True)
plt.show()
for scan in lidar.iter_scans():
if not plt.get_fignums():
break
update(plot, scan)
lidar.stop()
lidar.disconnect()
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
old_t = None
data = []
try:
print('Press Ctrl+C to stop')
for _ in lidar.iter_scans():
now = time.time()
if old_t is None:
old_t = now
continue
delta = now - old_t
print('%.2f Hz, %.2f RPM' % (1/delta, 60/delta))
data.append(delta)
old_t = now
except KeyboardInterrupt:
print('Stoping. Computing mean...')
lidar.stop()
lidar.disconnect()
delta = sum(data)/len(data)
print('Mean: %.2f Hz, %.2f RPM' % (1/delta, 60/delta))
ax.plot(x_y[1], x_y[0], "-" if args.plot_lines else "o")
elif args.front == 2:
ax.plot([-1 * (x) for x in x_y[0]], x_y[1], "-" if args.plot_lines else "o")
elif args.front == 3:
ax.plot([-1 * (y) for y in x_y[1]], [-1 * (x) for x in x_y[0]], "-" if args.plot_lines else "o")
elif args.front == 4:
ax.plot(x_y[0], [-1 * (y) for y in x_y[1]], "-" if args.plot_lines else "o")
# else:
# ax.plot(x_y[0], x_y[1], 'o') # This is just for debugging what is wrong with the above TODO
if __name__ == "__main__":
parse_args()
print args.front, type(args.front)
lidar = RPLidar(args.port_name)
lidar.connect()
print lidar.get_health()
print lidar.get_device_info()
lidar.start_scan()
time.sleep(2) # Let that baby warm up
set_plot()
plt.ion()
plt.show()
try:
while True:
point_list = [p for p in lidar._raw_points]
rp_point_list = [RPLidarPoint(raw_point) for raw_point in point_list]
#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"
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
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
MIN_SAMPLES = 180
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from roboviz import MapVisualizer
from scipy.interpolate import interp1d
import numpy as np
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 an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
