def main():
if 'linux' in os.name.lower():
lidar = RPLidar('/dev/ttyUSB0')
else:
lidar = RPLidar('com3')
# print(info(lidar))
# print(health(lidar))
lidar.stop_motor()
while 1 == 1:
print('----------------------------------------------')
print('1: Start Motor\n'
'2: Stop Motor\n'
'3: Start Scan\n'
'4: Stop Scan\n'
'5: Exit')
ans = input('Please Enter Menu Number: ')
if ans is '1':
start(lidar)
elif ans is '2':
stop(lidar)
elif ans is '3':
scan(lidar)
elif ans is '4':
print('4')
elif ans is '5':
break
else:
print('Invalid Option')
def run():
lidar = RPLidar(PORT_NAME)
lidar.reset()
#print statement required to capture value for vba shell
print("reset sent")
time.sleep(.1)
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 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)
line2 = 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)
ax.set_theta_zero_location('N', offset=0)
ax.set_theta_direction("clockwise")
iterator = lidar.iter_scans()
ani = animation.FuncAnimation(fig,
update_line,
fargs=(iterator, line),
interval=50)
plt.show()
lidar.stop()
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 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 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 _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()
def run():
lidar = RPLidar(PORT_NAME)
time.sleep(WAIT)
status, code = lidar.get_health()
time.sleep(WAIT)
lidar.disconnect()
print(status)
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 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(PORT_NAME)
iterator = lidar.iter_scans(400)
while (1):
update_line(50, iterator)
lidar.stop()
lidar.disconnect()
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():
"""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 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 __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 read_lidar_wrapper(angles,previous_readings):
''' Wrapper function for the RPLidar library.
This function takes as inputs:
-> the angles to measure as a list
-> the RPLidar object (instantiated using RPLidar library)
-> the previous_readings dictionary
This function returns:
-> the updated previous_readings dictionary with new measurements
'''
lidar = RPLidar('/dev/ttyUSB0') # Establish connection with Lidar
for i, scan in enumerate(lidar.iter_scans()): # Scan the sensor infinitely
if i>0:
readings = ({int(x[1]): int(x[2]) for x in list(scan) if int(x[1]) in angles})
break # Stop the scan to exit infinite loop
# Sometimes the sensor doesn't read all angles (unfortunately that's how it works)
# so we must add those missing values as the previous sensor readings
# to avoid having a dictionary with missing values
for key in previous_readings.keys():
try:
if readings[key]:
previous_readings[key]=readings[key]
except:
continue
lidar.stop() # Stop the sensor to be able to read it again
lidar.disconnect() # Stop the sensor to be able to read it again
return previous_readings
def connect():
global lidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
lidar.clear_input()
health = lidar.get_health()
print(health)
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 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 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 run(dis):
lidar = RPLidar(PORT_NAME)
try:
c = 0
for scan in lidar.iter_scans():
c = 0
d = 0
for (_, angle, distance) in scan:
if floor(angle) >= 178 and floor(
angle) <= 182 and distance != 0:
#print(floor(distance))
c = c + 1
d = d + floor(distance)
else:
continue
if c != 0:
if d / c <= dis:
return 1
except RPLidarException:
return 0
lidar.stop()
lidar.disconnect()
#run(10)
def run():
'''Main function'''
max_distance = 0
lidar = RPLidar(PORT_NAME)
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
if len(scan) > 100:
lcd.fill((0, 0, 0))
for (_, angle, distance) in scan:
max_distance = max([min([5000, distance]), max_distance])
radians = angle * pi / 180.0
x = distance * cos(radians)
y = distance * sin(radians)
point = (240 + int(x / max_distance * 159),
160 + int(y / max_distance * 159))
lcd.set_at(point, pygame.Color(255, 255, 255))
pygame.display.update()
print(max_distance)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
outfile.close()
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 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 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 run(self, n=320):
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.set_pwm(n)
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)
list.append(line + '\t' + str(ch.getPosition()) + '\n')
cnt += 1
if lidar.motor:
ln = 'Spinning %d'
else:
ln = 'Stopped %d'
print(ln % cnt)
if cnt > 85000:
break
except KeyboardInterrupt:
print('Stopping.')
except RPLidarException as e:
print(e)
print("complete")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
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')
words = line.split('\t')
word = float(words[2])
data.append(word)
except KeyboardInterrupt:
print('Stoping.')
'''i = 1
for mess in data:
if mess < 50:
print("value number {} is {}".format(i, mess) )
i += 1'''
lidar.stop()
lidar.disconnect()
outfile.close()
np.save(path, np.array(data))
def runLidar():
lidar = RPLidar(PORT_NAME)
iterator = lidar.iter_scans()
animateDataStream(iterator)
lidar.stop()
lidar.disconnect()
def LiDarProcess():
lidar = RPLidar(PORT_NAME)
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
CONTROL = [1.0, 1.0, 1.0, 1.0]
for point in scan:
ang = degreeFixer(-point[1]+270.0)
pra = distanceFilter(point[2], ang)
if ang < 90 or ang > 270:
if pra[0] < CONTROL[0]:
CONTROL[0] = pra[0]
if ang > 0 and ang < 180:
if pra[1] < CONTROL[1]:
CONTROL[1] = pra[1]
if ang > 90 and ang < 270:
if pra[0] < CONTROL[2]:
CONTROL[2] = pra[0]
if ang > 180 and ang < 360:
if pra[1] < CONTROL[3]:
CONTROL[0] = pra[1]
print("Process result: | X_P - %f | Y_P - %f | X_N - %f | Y_N - %f |" % (CONTROL[0], CONTROL[1], CONTROL[2], CONTROL[3]))
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
