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()
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
try:
print('Recording measurments... Press Crl+C to stop.')
try:
for measurment in lidar.iter_measurments():
# line = '\t'.join(str(v) for v in measurment)
# print(line + '\n')
if (measurment[2] > 0 and measurment[2] < 90):
if (measurment[3] < 1000 and measurment[3] > 100):
print("Angle: ", measurment[2] - angleoffset)
print("Distance: ", measurment[3])
steer(measurment[2] - angleoffset)
except KeyboardInterrupt:
print('Stopping.')
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
c.send("motors", 0, 0, 0) # turn off wheel motors
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 getCoord(self):
'''Main function'''
lidar = RPLidar(self.rpLidar_port)
lidar.stop()
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
lidar.stop()
try:
print('Recording measurments... Press Crl+C to stop.')
for i in range (0,10):
scan = next(lidar.iter_scans())
for measurment in scan:
lidar.stop()
line = '\t'.join(str(v) for v in measurment)
#print(line)
lidar.stop()
time.sleep(0.0625)
#for scan in lidar.iter_scans():
# print(scan)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
def main():
circle = np.zeros(360)
lidar = RPLidar("/dev/ttyUSB0")
time.sleep(2)
go = 1
#try:
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
iterator = lidar.iter_scans()
obstacle = []
sectors = [[], [], [], [], [], []]
try:
while True:
obstacles, sectors = lidarDistanceAndSector(next(iterator))
if obstacles:
autoSpeed(sectors)
autoSteer(sectors)
print('finished cycle')
except:
print("error occured in main")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def tu_lidar(_argv):
lidar = RPLidar('/dev/ttyUSB0')
# check lidar state and health
print(lidar.get_info())
print(lidar.get_health())
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=800, min_len=5)):
print(i, len(scan))
valid_measurements = np.array([])
for measurement in scan:
quality, angle, distance = measurement
if distance != 0:
x = distance * np.cos(convert_degrees_to_rad(angle))
y = distance * np.sin(convert_degrees_to_rad(angle))
cartesian = np.array([x, y])
measurement = np.vstack((measurement, cartesian))
if measurement.shape[0]:
plot_measurement(measurement)
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def test_rp_lidar():
from rplidar import RPLidar
import serial
import glob
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)
except serial.SerialException:
pass
print("available ports", result)
lidar = RPLidar(result[0], baudrate=115200)
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print(f'{i}: Got {len(scan)} measurements')
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def main():
lidar = RPLidar(PORT)
# check RPLidar
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
# setup value
points = np.array([[], []])
previous = np.array([[], []])
rotate = IDENTITY
slide = np.array([0, 0])
grid_x = np.linspace(MIN_RANGE, MAX_RANGE, GRID_POINTS)
grid_y = np.linspace(MIN_RANGE, MAX_RANGE, GRID_POINTS)
grid = cal_positive_grid(points, grid_x, grid_y)
fig = plt.figure()
# loop while mapping
for i, scan in enumerate(lidar.iter_scans(min_len=60)):
raw = filter(lambda element: element[1] >= 270 or element[1] <= 90,
scan)
points = transform_raw(raw)
pre, cur = random_filter(previous, points)
if len(pre) > 0 and len(cur) > 0 and i % 5 == 0:
is_change = False
rotation, transform = icp(pre.T, cur.T)
rotation_to_degrees(rotation)
if rotation_to_degrees(rotation) >= 1:
rotate = np.dot(rotation, rotate)
is_change = True
if math.sqrt(transform[0]**2 + transform[1]**2) >= 5:
slide = slide + np.dot(rotate, transform)
is_change = True
if is_change:
previous = points
grid = update_grid(grid,
points.T,
GRID_LENGTH,
grid_x,
grid_y,
origin=slide,
rotation=rotate)
# make occupancy grid
# illustrated
if i % 10 == 0:
occupancy_grid = cal_occupancy_grid(grid)
plt.clf()
plt.pcolormesh(grid_x, grid_y, occupancy_grid)
plt.plot(slide[1], slide[0], 'ro')
tracker = np.dot(rotate, TRACKER)
plt.plot(slide[1] + tracker[1], slide[0] + tracker[0], 'bo')
plt.pause(0.01)
if i == 2:
previous = points
# stop RPlidar
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
# file save
save_override_map(occupancy_grid)
def connect(addr):
lidar = RPLidar(addr)
info = lidar.get_info()
#print(info)
health = lidar.get_health()
#print(health)
return lidar
def connect():
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
return lidar
class RpLidar:
# Constructor
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 runLidar(self):
self.iterator = self.lidar.iter_scans(max_buf_meas=10000)
def stopLidar(self):
self.lidar.stop()
self.lidar.disconnect()
def GetObstacles(self):
scan = next(self.iterator)
obstacles = np.array([[80, 70]])
for y in scan:
# STEP 1: extract polar coordonates from scan
pPolar = [math.radians(y[1]), y[2]] # [angle in radian, distance]
# STEP 2: compute cartesian coordonates
angle = pPolar[0]
passageMatrix = np.array([[np.cos(angle),
np.sin(angle)],
[(np.sin(angle)) * -1,
np.cos(angle)]])
distanceMatrix = np.array([[pPolar[1]], [0]])
pCart = np.dot(passageMatrix, distanceMatrix)
# STEP 4: save the obstacles inside an array
obstacles = np.append(obstacles, [[pCart[0, 0], pCart[1, 0]]],
axis=0)
return obstacles
def ScanEnvironmemnt(self):
cmd = b'\x20' # SCAN_BYTE
self.lidar._send_cmd(cmd)
dsize, is_single, dtype = self.lidar._read_descriptor()
while True:
raw = self.lidar._read_response(dsize)
print(raw)
time.sleep(5)
def main():
flag = False
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
health = lidar.get_health()
print(info, '\n', health)
cap = cv2.VideoCapture(2)
if flag:
roi, mapx, mapy = Calibration(cap)
else:
fs = cv2.FileStorage("callibration.xml", cv2.FILE_STORAGE_READ)
if not (fs.isOpened()):
return 0
roi = fs.getNode('roi').mat()
mapx = fs.getNode('mapx').mat()
mapy = fs.getNode('mapy').mat()
fs.release()
x, y, w, h = roi
hog = cv2.HOGDescriptor()
#svm = ml.SVM_create()
#ml.SVM_load('/HOGlegs70x134.xml')
#hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())
hog.load('/home/vladimir/PycharmProjects/PickToGo/HOGlegs.xml')
#hog.load('/HOGlegs70x134.xml')
while True:
#RunLidar(lidar)
ret, frame = cap.read()
gframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gframe = cv2.remap(gframe, mapx, mapy, cv2.INTER_LINEAR)
gframe = gframe[int(y[0]):int(y[0] + h[0]), int(x[0]):int(x[0] + w[0])]
orig, frame = DetectUsingHOGdetector(hog, gframe)
# upper_bodies = DetectUsingHaar(gframe)
# for (x1, y1, w1, h1) in bodies:
# cv2.rectangle(frame, (x1, y1), (x1+w, y1+h), (0, 255, 0), 2)
# for (x1, y1, w1, h1) in upper_bodies:
# cv2.rectangle(frame, (x1, y1), (x1+w, y1+h), (255, 0, 0), 2)
# for (x1, y1, w1, h1) in lower_bodies:
# cv2.rectangle(frame, (x1, y1), (x1 + w, y1 + h), (0, 0, 255), 2)
cv2.imshow('Detection', frame)
cv2.imshow('Hog', orig)
if cv2.waitKey(1) & 0xFF == ord('q'):
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
break
def lidar_logger(output_directory):
""" LIDAR Logger """
global LIDAR_STATUS, LIDAR_DATA
port_name = '/dev/lidar'
lidar = None
# Configure
config = util.read_config()
while not util.DONE:
try:
lidar = RPLidar(port_name)
print(lidar.get_info())
print(lidar.get_health())
# Open the output file
timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M")
with open(os.path.join(output_directory,timestamp+"_lidar.csv"), "w") as lidar_output:
lidar_output.write("%s %s %s *\n" % (config['time'], "VERSION", json.dumps({"class": "VERSION", "version": util.DATA_API})))
lidar_output.write("%s %s %s *\n" % (config['time'], config['class'], json.dumps(config)))
for _, scan in enumerate(lidar.iter_scans(max_buf_meas=1500)):
lidartime = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ")
data = []
for (_, angle, distance) in scan:
if distance > 0:
data.append((int(angle)%360, int(distance)))
lidar_data = {
'class': 'LIDAR',
'device': 'A1M8',
'time': lidartime,
'scan': data,
}
lidar_output.write("%s %s %s *\n" % (lidar_data['time'], lidar_data['class'], json.dumps(lidar_data)))
LIDAR_DATA = lidar_data
LIDAR_STATUS = True
if util.DONE:
break
except KeyboardInterrupt:
util.DONE = True
except Exception as ex:
print("LIDAR Logger Exception: %s" % ex)
time.sleep(util.ERROR_DELAY)
if lidar is not None:
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
LIDAR_STATUS = False
time.sleep(util.ERROR_DELAY)
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
try:
scan(lidar)
except KeyboardInterrupt:
stop = True
print('Stopping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run():
lidar = RPLidar(PORT_NAME)
lidar.stop()
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
print(lidar)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
print(dir(lidar))
def health_check():
lidar = RPLidar(PORT_NAME)
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return
def run():
'''Main function'''
global stop
if stop == False:
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
ioloop = asyncio.get_event_loop()
tasks = [ioloop.create_task(scan(lidar)), ioloop.create_task(drive())]
try:
ioloop.run_until_complete(asyncio.wait(tasks))
except KeyboardInterrupt:
print('Stopping.')
stop = True
lidar.stop()
lidar.stop_motor()
c.send("motors", 0, 0, 0) # turn off wheel motors
lidar.disconnect()
ioloop.close()
class Lidar(Thread):
def __init__(self, port, baud, data, limit=500):
Thread.__init__(self)
self.lidar = RPLidar(port=port, baudrate=baud)
try:
print(self.lidar.get_info())
except Exception as e:
print(e)
self.lidar.eteindre()
self.lidar.reset()
self.lidar.connect()
self.lidar.start_motor()
self.data = data
self.limit = limit
def run(self):
sleep(1)
try:
for measurment in self.lidar.iter_measurments(
max_buf_meas=self.limit):
s = len(self.data[0])
if s >= self.limit:
self.data[0].pop(0)
self.data[1].pop(0)
self.data[0].append(measurment[2] * pi / 180) #agle en rad
self.data[1].append(measurment[3] / 1000) #distance en m
except Exception as error:
print(error)
def join(self, timeout=None):
self.eteindre()
Thread.join(self, timeout)
def eteindre(self):
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
time.sleep(1)
info = lidar.get_info()
print(info)
drive(0) # start going forward
lasttime = int(time.time() * 1000)
try:
counter = 0
print('Recording measurements... Press Crl+C to stop.')
data = 0
try:
for measurment in lidar.iter_measurments():
if ((int(time.time() * 1000) - lasttime) < 1000):
raise ValueError # do this loop at 10Hz
if (measurment[2] > 0
and measurment[2] < 90): # in angular range
if (measurment[3] < 1000
and measurment[3] > 100): # in distance range
data = data + measurment[2] # angle
counter = counter + 1 # increment counter
except ValueError:
print("this should happen ten times a second")
lasttime = int(time.time() * 1000) # reset 10Hz timer
if counter > 0: # this means we see something
average_angle = data / counter
print("Average Angle: ", average_angle - angleoffset)
drive(average_angle - angleoffset)
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
c.send("motors", 0, 0, 0) # turn off wheel motors
lidar.disconnect()
class Lidar:
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 start(self):
for i, scan in enumerate(self.lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 100:
break
self.stop()
def stop(self):
self.lidar.stop()
#self.lidar.stop_motor()
#self.lidar.disconnect()
import socket
import time
import cv2
from imutils.video import VideoStream
import imagezmq
from rplidar import RPLidar
print('Connecting to webcam...')
stream = cv2.VideoCapture(4) #2
print("Connecting to lidar...")
lidar = RPLidar('/dev/ttyUSB0')
print("")
print("-------------------LiDAR-------------------")
print(lidar.get_info())
print("-------------------------------------------")
print("")
print('Connecting to headset - streaming using ZMQ...')
min_angle = 39
max_angle = 360 - min_angle
# The IP address below is the IP address of the headset computer.
sender = imagezmq.ImageSender(
connect_to='tcp://localhost:5556') #tcp://192.168.0.101:5556
time.sleep(2.0)
jpeg_quality = 35 #0 to 100
resolution_width = 640
class RPLiDAR:
def __init__(self, sectors):
# self.lidar = RPLidar("\\\\.\\com4") # Check to see if this runs on mac
self.lidar = RPLidar("/dev/ttyUSB0")
# laptop. If not make change. May be the /dev/ thing
self.sectors = sectors
self.sector_space = {}
self.prev_sector_space = {}
time.sleep(1)
info = self.lidar.get_info()
health = self.lidar.get_health()
print(info, health, sep='\n')
self.file = open('lidar-data.csv', 'w')
self.writer = csv.writer(self.file)
def scan_area(self, limit=100):
for i, scan in enumerate(self.lidar.iter_scans()):
if i > limit:
break
print('%d: Got %d measurements' % (i, len(scan)))
sector_space = np.zeros(6)
warning_flag = False
for j in scan:
k = math.floor(j[1] / 60)
if j[2] < 1000:
# print("Warning: object at %f degrees is too close"
# % (j[1]))
sector_space[k] += -math.inf # Set to say space is unsafe
warning_flag = True
else:
sector_space[k] += j[2] # adding distance to sector array
if warning_flag:
print(sector_space) # outputs six values
print("Object(s) are too close...")
free_space = max(sector_space)
if free_space < 1000:
print("There is nowhere safe to venture, immediate landing"
" to avoid damage...")
break
evacuation_direction = \
(sector_space.index(free_space)+1) * 60 - 30
print("Evacuate in the direction of %d degrees" %
evacuation_direction) # This is the safest spot
def area_report(self, limit=100):
for i, scans in enumerate(self.lidar.iter_scans()):
if i > limit:
# self.stop()
break
self.sector_space = {}
divisor = 360 // self.sectors
for scan in scans:
section = math.floor(scan[1] / divisor)
try:
self.sector_space[section] = np.append(
self.sector_space[section], scan[2])
except KeyError:
self.sector_space[section] = np.array(scan[2])
evaluation_space = self._evaluate_spcae()
# print('evaluate space at ', time.time(), evaluation_space,
# file=self.file)
# self.file.write('evaluate space \n' + str(evaluation_space))
direction = self._get_direction(evaluation_space)
if direction == -1:
print('There are no safe regions!')
print('Go to region %d' % direction)
self._write_file(evaluation_space, direction)
def _evaluate_spcae(self):
evaluation = []
for i in range(self.sectors):
try:
section = self.sector_space[i]
evaluation.append((i, np.min(section), np.max(section),
np.average(section), section))
except KeyError:
evaluation.append((None, i, None, None, None))
return evaluation
def _get_direction(self, evaluation_space):
current_section = -1
previous_min = 1
for value in evaluation_space:
# print(value)
section, min, max, average, sector = value
try:
if min > previous_min:
current_section = section
previous_min = min
except TypeError:
pass
return current_section
def _write_file(self, evaluation_space, direction):
self.writer.writerow('sector number: %d' % direction)
for values in evaluation_space:
self.writer.writerow((datetime.datetime.time(
datetime.datetime.now()).strftime('%H:%M:%S'), values))
def stop(self):
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
self.file.close()
import pybullet as p
import time
import math
import time
from rplidar import RPLidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
time.sleep(1)
print("lidar info:", lidar.get_info())
numRays = 5000
iterator = lidar.iter_scans()
useGui = True
if (useGui):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.loadURDF("samurai.urdf")
#p.loadURDF("r2d2.urdf",[3,3,1])
rayFrom = []
rayTo = []
rayIds = []
rayLen = 0.1
def connect(self):
lidar = RPLidar(self.addr) # demarre le lidar
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
from rplidar import RPLidar
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
class RPLidarMQTTBridge(object):
health_state = 0
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 generate_topic(self, subtopic):
return self.topic_prefix + subtopic
def clear_rplidar_readings(self):
self.mqtt.publish(self.generate_topic('/measurement'), None, 1, True)
def clear_rplidar_health(self):
self.mqtt.publish(self.generate_topic('/health'), None, 1, True)
def report_rplidar_measurement(self, measurement):
# Only publish valid measurements
if self.measurement_valid(measurement):
reading = dict(quality=measurement[1],
angle=measurement[2],
distance=measurement[3],
timestamp=iso8601_timestamp())
if measurement[3] == 0:
print("logging invalid distance")
self.publish_event(reading=reading)
def report_rplidar_info(self, info):
self.publish_event_raw(topic='/info/model', reading=info['model'])
self.publish_event_raw(topic='/info/hardware',
reading=info['hardware'])
self.publish_event_raw(
topic='/info/firmware',
reading=("%d.%d" % (info['firmware'][0], info['firmware'][1])))
self.publish_event_raw(topic='/info/serialnumber',
reading=info['serialnumber'])
def report_rplidar_source(self):
self.publish_event_raw(topic='/source', reading="rplidar_mqtt_bridge")
def report_rplidar_health(self, health):
self.publish_event(reading=health, topic='/health')
@staticmethod
def measurement_valid(measurement):
if len(measurement) != 4:
return False
# In the case of an invalid measurement, the distance and quality are both set to 0
if measurement[1] == 0:
return False
if measurement[3] == 0.0:
return False
return True
def publish_event(self, reading, topic='/measurement', raw=False):
logging.debug(reading)
if raw is True:
data = reading
else:
data = json.dumps(reading, sort_keys=False)
self.mqtt.publish(self.generate_topic(topic), data)
def publish_event_raw(self, reading, topic='/measurement'):
self.publish_event(reading, topic, raw=True)
def poll(self):
self.mqtt.loop_start()
try:
health = self.lidar.get_health()[0]
if self.health_state != health:
self.report_rplidar_health(health)
for measurement in self.lidar.iter_measurments():
self.report_rplidar_measurement(measurement)
except KeyboardInterrupt:
pass
finally:
self.cleanup()
def cleanup(self):
self.lidar.stop()
self.lidar.disconnect()
self.clear_rplidar_health()
self.mqtt.disconnect()
self.mqtt.loop_stop()
class LidarReader(threading.Thread):
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 terminate(self):
self._running = False
def publish_data(self):
self.message_bus.send(
self.lidar_updates_topic_name,
Message(Message.lidar_data, self.lidar_data.tolist()))
#self.config.log.info('lidar publish to (%s)' % self.lidar_updates_topic_name)
def run(self):
self.config.log.info('The lidar processor is creating topic: %s' %
self.lidar_updates_topic_name)
self.message_bus.create_topic(self.lidar_updates_topic_name)
try:
self.config.log.info("waiting for lidar to start")
time.sleep(5)
self.lidar_controller.clear_input()
info = self.lidar_controller.get_info()
self.config.log.info(
"lidar info: Model:{} Firmware:{} Hardware:{} SN:{}".format(
info['model'], info['firmware'], info['hardware'],
info['serialnumber']))
health = self.lidar_controller.get_health()
self.config.log.info(
"lidar health: Status:{} Error Code:{}".format(
health[0], health[1]))
self.config.log.info("started reading loop...")
# average N measurments per angle
num_scans = 0
data = np.zeros((360, 2), dtype=int)
for measurment in self.lidar_controller.iter_measurments():
if not self._running:
self.lidar_controller.stop_motor()
print("*****************************************")
break
new_scan, quality, angle, distance = measurment
if (distance > 0 and quality > 5):
theta = min(int(np.floor(angle)), 359)
data[theta, 0] += distance
data[theta, 1] += 1
if new_scan: num_scans += 1
if num_scans > 10:
with np.errstate(divide='ignore', invalid='ignore'):
mean_distance = data[:, 0] / data[:, 1]
# interpolate nan's
mask = np.isnan(mean_distance)
mean_distance[mask] = np.interp(np.flatnonzero(mask),
np.flatnonzero(~mask),
mean_distance[~mask])
np.copyto(self.lidar_data, mean_distance)
self.publish_data()
self.event.set()
# reset accumulators
data = np.zeros((360, 2), dtype=int)
num_scans = 0
except (KeyboardInterrupt, SystemExit):
# this is not working... neeed to move inside rplidar code?
self.lidar_controller.stop()
self.lidar_controller.stop_motor()
self.lidar_controller.disconnect()
raise
finally:
self.lidar_controller.stop_motor()
self.lidar_controller.stop()
self.lidar_controller.disconnect()
return
def lidar_logger(output_directory):
global DONE, LIDAR_STATUS, LIDAR_DATA
port_name = '/dev/lidar'
lidar = None
# Configure
try:
with open("config.json", "r") as f:
config = json.loads(f.read())
except:
config = {"class": "CONFIG", "lidar": {"log": True}}
config['time'] = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ")
# Create the output directory
if not os.path.isdir(output_directory):
os.mkdir(output_directory)
while not INLOCSYNC:
time.sleep(SYNC_DELAY)
while not DONE:
try:
lidar = RPLidar(port_name)
mylog(lidar.get_info())
mylog(lidar.get_health())
# Open the output file
timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M")
with open(os.path.join(output_directory, timestamp + "_lidar.csv"),
"w") as lidar_output:
lidar_output.write("#v%d\n" % VERSION)
lidar_output.write(
"%s %s %s *\n" %
(config['time'], config['class'], json.dumps(config)))
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=1500)):
if INLOCSYNC or ALWAYS_LOG:
lidartime = datetime.datetime.now().strftime(
"%Y-%m-%dT%H:%M:%S.%fZ")
data = []
for (_, angle, distance) in scan:
if distance > 0:
data.append((int(angle) % 360, int(distance)))
lidar_data = {
'class': 'LIDAR',
'device': 'A1M8',
'time': lidartime,
'scan': data,
}
lidar_output.write(
"%s %s %s *\n" %
(lidar_data['time'], lidar_data['class'],
json.dumps(lidar_data)))
LIDAR_DATA = lidar_data
LIDAR_STATUS = True
if DONE:
break
except KeyboardInterrupt:
DONE = True
except Exception as ex:
mylog("LIDAR Logger Exception: %s" % ex)
time.sleep(ERROR_DELAY)
if lidar is not None:
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
LIDAR_STATUS = False
time.sleep(ERROR_DELAY)
from rplidar import RPLidar
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_info()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i,len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
class LIDAR_Interface(Thread):
# These are all in Hz
_MAX_SCAN_RATE = 10
_MIN_SCAN_RATE = 0
_MAX_SCAN_PWM = 1023
_MIN_SCAN_PWM = 0
_DEFAULT_SCAN_RATE = 5.5
_GENERATOR_BUFF_SIZE = 500
_ANGULAR_TOLERANCE = 2
def __init__(self, loc: str = "/dev/ttyUSB1", sample_rate: float = 4000, scan_rate: float = 5.5, stack_depth:int = 10):
self.__lidar = RPLidar(port=loc)
super(LIDAR_Interface, self).__init__()
self.__min_parsable = 5
self.__sample_rate = sample_rate
self.__scan_rate = scan_rate
self.__samples_per_rev = LIDAR_Interface._GENERATOR_BUFF_SIZE # this may change after testing
self.__iter_scan = self.__lidar.iter_scans(self.__samples_per_rev)
self.__stack = Stack(stack_depth)
self.__current_scan = []
self.__prev_scan = []
self._max_distance = 5000
self._min_distance = 0
self.__running = False
atexit.register(self.exit_func)
# control functions
def stop_thread(self):
self.__running = False
def exit_func(self):
self.stop_thread()
self.stop_motor()
self.stop_sensor()
self.__lidar.disconnect()
def stop_sensor(self):
self.__lidar.stop()
def stop_motor(self):
self.__lidar.stop_motor()
def reset_sensor(self):
self.__lidar.reset()
self.__lidar.clear_input()
def start_motor(self):
self.__lidar.start_motor()
@property
def running(self):
return self.__running
# properties
@property
def max_distance(self):
return self._max_distance
@max_distance.setter
def max_distance(self, distance: int):
if distance > self._min_distance:
if distance < 5000:
self._max_distance = distance
@property
def min_distance(self):
return self._min_distance
@min_distance.setter
def min_distance(self, distance: int):
if distance >= 0:
if distance < self._max_distance:
self._min_distance = distance
@property
def sensor_health(self):
return self.__lidar.get_health()
@property
def sensor_info(self):
return self.__lidar.get_info()
@property
def sample_rate(self):
return self.__sample_rate
@property
def scan_rate(self):
return self.__scan_rate
@scan_rate.setter
def scan_rate(self, value: float):
if LIDAR_Interface._MAX_SCAN_RATE >= value >= LIDAR_Interface._MIN_SCAN_RATE:
self.__scan_rate = value
self.__sample_rate = LIDAR_Interface._GENERATOR_BUFF_SIZE * self.__scan_rate
self._set_scan_rate()
@property
def pop_recent_scan(self) -> list:
if self.__stack.length > 0:
return self.__stack.pop()
return list()
@property
def recent_scan(self) -> list:
if self.__stack.length > 0:
return self.__stack.top
return list()
@property
def stack(self) -> Stack:
return self.__stack
# conversion function
@staticmethod
def _map(x: float, from_min: float, from_max: float, to_min: float, to_max: float) -> float:
return (x - from_min) * (to_max - to_min) / ((from_max - from_min) + to_min)
# Motor control functions
def _set_scan_rate(self):
self.__lidar.set_pwm(self._map(self.__scan_rate, LIDAR_Interface._MIN_SCAN_RATE, LIDAR_Interface._MAX_SCAN_RATE,
LIDAR_Interface._MIN_SCAN_PWM, LIDAR_Interface._MAX_SCAN_PWM))
# thread function
def start(self) -> None:
super(LIDAR_Interface, self).start()
self.__running = True
def run(self) -> None:
while self.__running:
# iter must produce a full rotation (360 points) before we can use it
#_scan = self.__lidar.iter_scans(min_len=self.__samples_per_rev)
if self.__iter_scan.__sizeof__() > 0:
_scan = next(self.__iter_scan)
for scan in _scan:
current = Ray(scan[2], scan[1], scan[0])
if current.radius > self._min_distance:
if current.radius < self._max_distance:
self.__current_scan.append(current)
# if the current scan has the total points for a rotation we can consume it and reset the current scan
if self.__current_scan.__len__() >= 360:
self.__stack.push(self.__current_scan.copy())
self.__current_scan.clear()
self.__lidar.stop()
self.__lidar.stop_motor()
self.__lidar.disconnect()
