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 repere(balises, depx, depy, theta):
lidar = RPLidar('/dev/ttyUSB0')
abscisse = []
ordonnee = []
x_v = []
y_v = []
for i, scan in enumerate(lidar.iter_measurments()):
#print(i,scan)
valeur = scan[1]
angle = scan[2] * 2 * np.pi / 360
x = -scan[3] * np.cos(angle + np.pi / 2)
y = scan[3] * np.sin(angle + np.pi / 2)
x_v.append(x)
y_v.append(y)
if valeur == 15 and test_zone(x, y, balises):
abscisse.append(x)
ordonnee.append(y)
if i >= 2000:
lidar.stop()
lidar.disconnect()
balise1, balise2, balise3, balise4 = pos_balises(
abscisse, ordonnee, balises, depx, depy, theta)
return (balise1, balise2, balise3, balise4)
def GetDistances():
lidar = RPLidar(PORT_NAME)
lidar.set_pwm(500)
print("move unit now!")
time.sleep(10)
print("measuring new location!")
for measurment in lidar.iter_measurments():
# convert angle and distance to x and y
if measurment[1] == 15 and measurment[3] < nMaxRange and measurment[
3] > nMinRange:
# finding the 2 missing legs given a hypotenuse(distance) and angle in degrees from the lidar
if round(measurment[2], 0) in [359, 0, 1]:
# print("north ", measurment[2], measurment[3] )
nCurNorth = measurment[3]
if round(measurment[2], 0) in [89, 90, 91]:
# print("East ", measurment[2], measurment[3])
nCurEast = measurment[3]
if round(measurment[2], 0) in [179, 180, 181]:
# print("south ", measurment[2], measurment[3])
nCurSouth = measurment[3]
if round(measurment[2], 0) in [269, 270, 271]:
# print("West ", measurment[2], measurment[3])
nCurWest = measurment[3]
if nCurNorth != 0 and nCurEast != 0 and nCurSouth != 0 and nCurWest != 0:
break
print("4 directions NESW ", nCurNorth, nCurEast, nCurSouth, nCurWest)
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()
import math
import time
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
from rplidar import RPLidar
x = []
y = []
counter = 0
os.system('sudo chmod 666 /dev/ttyUSB0')
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
for measurment in lidar.iter_measurments():
line = '\n'.join(str(v) for v in measurment)
newline = line.split("\n")
theta = float(newline[2])
distance = float(newline[3])
if (distance > 0):
x_coord = math.cos(math.radians(theta)) * distance
y_coord = math.sin(math.radians(theta)) * distance * -1
y.append(y_coord)
x.append(x_coord)
counter = counter + 1
if (counter == 1000):
break
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
print("Connecting to lidar...")
lidar = RPLidar(PORT_NAME)
print(lidar.get_info())
resolution_width = 640
resolution_height = int(0.75 * resolution_width)
min_angle = 39
max_angle = 360 - min_angle
field_of_view = max_angle - min_angle
degree_pixels = resolution_width / field_of_view
distances = {}
while True:
for i, measurement in enumerate(lidar.iter_measurments()):
angle = int(measurement[2])
distance = measurement[3]
print(distance)
try:
ret_val, frame = cam.read()
frame = cv2.resize(frame, (resolution_width, resolution_height))
if (angle < min_angle or angle > max_angle):
distances[angle] = distance
for angle in distances:
cv2.circle(frame, (angle, int(resolution_height / 2)), 3,
(0, 255, 0), 5)
cv2.imshow('frame', frame)
PORT = 10002
numberOfScans = 0
#start_time = time.time()
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.bind((HOST, PORT))
s.listen()
conn, addr = s.accept()
with conn:
data = int.from_bytes(conn.recv(1024), byteorder=sys.byteorder)
print("Data:", data)
while data == 1:
print("entered the while loop")
for i in lidar.iter_measurments(max_buf_meas=10000):
if i.__getitem__(3) != 0:
if time.process_time() % 2 <= 0.1:
newScan = True
numberOfScans += 1
print("sent:" + str(numberOfScans) +
"scans, at a process time of:" +
str(time.process_time()))
else:
newScan = False
output = str(i.__getitem__(3)) + "," + str(
i.__getitem__(2)) + "," + str(
newScan) #+ str(i.__getitem__(0))
print('Bring up CAN0....')
os.system("sudo ifconfig can0 down")
time.sleep(0.1)
os.system("sudo /sbin/ip link set can0 up type can bitrate 400000")
time.sleep(0.1)
try:
bus = can.interface.Bus(channel='can0', bustype='socketcan_native')
except OSError:
print('Cannot find PiCAN board.')
exit()
try:
print('Recording measurments... Press Crl+C to stop.')
#print('Recording measurments...')
for new_scan, quality, angle, distance in lidar.iter_measurments():
#Filtrage des donnees recuperees par le lidar
if quality != 0 and 172 <= angle and angle <= 188:
if quality > bestQuality:
bestQuality = quality
if distance < bestDistance:
bestDistance = int(distance)
i = 1
#Calcul de la vitesse
elif angle > 198 and i == 1:
#Refaire les mesures de vitesse pour obtenir un meilleur coefficient
errorDistance = bestDistance - 2000
variationError = errorDistance - previousErrorDistance
boost = (errorDistance * Kp) + (Kd * variationError)
previousErrorDistance = errorDistance
#!/usr/bin/env python2
import matplotlib.pyplot as plt
from rplidar import RPLidar
from array import *
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for scan in lidar.iter_measurments():
print(scan[0])
print("\n")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
from ObjectDetector import ObjectDetector
from rplidar import RPLidar
import time
obd = ObjectDetector(ObjectDetector.gen_segments(
right = ObjectDetector.segment_range(30, 90),
front = ObjectDetector.segment_range(-30, 30),
left = ObjectDetector.segment_range(-90, -30)
))
try:
lidar = RPLidar('/dev/ttyUSB0')
print('Initializing')
time.sleep(5)
print('Collecting data')
for data in lidar.iter_measurments(max_buf_meas=800):
if obd.update(data):
print(obd.detect())
except KeyboardInterrupt:
pass
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run(usb):
'''Main function'''
PORT_NAME = '/dev/ttyUSB'
PORT_NAME = PORT_NAME + str(usb)
lidar = RPLidar(PORT_NAME)
limit = 45.0
sector = []
bounds = np.zeros(8)
count = 0
start = 0
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
one_scan = np.asarray(measurment)
distance_warning = 10 # mm
dist = one_scan[3]
angle = float(one_scan[2])
if (angle < limit+ 10) and (angle > limit - 55):
sector = np.append(sector, dist)
start = 1
else:
if start ==1:
sector_avg = np.average(sector)
bounds[(limit / 45) - 1] = sector_avg
limit = limit + 45
sector = []
mask = bounds < distance_warning
bounds_logic = np.zeros(8, dtype=int)
for i in range(0, 7):
if bounds[i] < distance_warning:
bounds_logic[i] = 1
file = open("testfile.txt", "w")
mask_to_write = np.array2string(bounds_logic)
file.write(mask_to_write)
print(mask_to_write)
for i in range(1,20):
print('\r')
#sys.stdout.write(mask_to_write)
if limit > 360:
limit = 45
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.stop()
lidar.disconnect()
x, y, z = 0, 0, 0
lidar = None
for n in range(0, 4):
try:
port = '/dev/ttyUSB%s' % n
print('Searchin RPLidar on port %s ... ' % port, end = '', flush = 1)
lidar = RPLidar(port)
lidar.connect()
print('done')
break
except RPLidarException:
print('fail!')
out = []
for new_scan, quality, φ, ρ in lidar.iter_measurments():
out += [(new_scan, quality, φ, ρ)]
if len(out) > 1000:
break
dump(out, 'scan.json')
raise Exception
def normalize(layer):
rez = []
for φ0 in range(360):
Δφ_min = 360
out = ()
for z, φ, ρ in layer:
Δφ = abs(φ -φ0)
# docs: https://rplidar.readthedocs.io/en/latest/
from rplidar import RPLidar
import time
import threading
import sys
lidar = RPLidar('/dev/ttyUSB0')
stop_flag = False
def close_lidar():
stop_flag = True
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
sys.exit(0)
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
time.sleep(2) #set 2 seconds to get full speed
timer = threading.Timer(3.0, close_lidar) #set timer for 3 seconds
timer.start()
for measurment in enumerate(lidar.iter_measurments()): #print measurements
print("quality: ",measurment[1][1],"angle: ", measurment[1][2], "distance: ",measurment[1][3] )
class Lidar_thread(Thread):
def __init__(self, bus):
Thread.__init__(self)
self.bus = bus
print(self.getName(), 'initialized')
self.lidar = RPLidar(PORT_NAME)
def run(self):
print(self.getName(), 'running')
i = 0
bestQuality = 0
bestDistance = 7000
oldGoodValue = 7000
Kp = 0.06
for new_scan, quality, angle, distance in self.lidar.iter_measurments(
):
#print(self.getName(), 'reading lidar') #OK
if VN.exit_lidar.isSet():
print(self.getName(), 'stopping')
# dans VarNairobi exit_lidar=threading.Even(); exit_lidar.clear()
# et dans le main on ajoute signal.signal(signal.sigint signal handler)???
time.sleep(5)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
break
else:
#dans VarNairobi wait_interface_on=threading.Even(); wait_interface_on.clear()
#Filtrage des donnees recuperees par le lidar
if quality != 0 and 172 <= angle and angle <= 188:
if quality > bestQuality:
bestQuality = quality
if distance < bestDistance:
bestDistance = int(distance)
print(self.getName(), ': BD : ', bestDistance)
i = 1
#Calcul de la cmd vitesse
elif angle > 198 and i == 1:
print(self.getName(), ': Test')
if VN.DistLidarSem.acquire(
False): #acquire semaphore without blocking
print(self.getName(), ': access DistLidar')
VN.DistLidar = bestDistance
VN.DistLidarSem.release()
else:
print(self.getName(), ': can not access DistLidar')
if VN.PlatooningActive.isSet():
if bestDistance > 4000:
if oldGoodValue < 3000:
temp = bestDistance
bestDistance = oldGoodValue
oldGoodValue = temp
else:
oldGoodValue = bestDistance
else:
oldGoodvalue = bestDistance
errorDistance = bestDistance - 2000
speed = (errorDistance * Kp)
speed = int(speed)
if speed <= 0:
speed = 0
elif speed >= 20:
speed = 20
if bestDistance >= 3000:
speed = 0
cmd_mv = (50 + speed) | 0x80
print(cmd_mv)
msg = can.Message(
arbitration_id=MCM,
data=[cmd_mv, cmd_mv, 0, 0, 0, 0, 0, 0],
extended_id=False)
self.bus.send(msg)
i = 0
bestQuality = 0
bestDistance = 7000
