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():
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))
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 > 100:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
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()
# We will use this to store previous scan in case current scan is inadequate
previous_distances = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extrat (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
theta = 0
r = 0
counter = 0
fieldnames = ["x_value", "theta", "r"]
with open('data.csv', 'w') as csv_file:
csv_writer = csv.DictWriter(csv_file, fieldnames=fieldnames)
csv_writer.writeheader()
with open('data.csv', 'a') as csv_file:
csv_writer = csv.DictWriter(csv_file, fieldnames=fieldnames)
print('test1')
print('test2')
print(x_value, theta, r)
for scan in lidar.iter_scans(max_buf_meas=500):
print('test3')
for data in scan:
counter = counter + 1
x_value = str(data[0])
theta = str(data[1])
r = str(data[2])
info = {"x_value": x_value, "theta": theta, "r": r}
csv_writer.writerow(info)
if counter > 200:
csv_file.truncate()
class Robot:
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 = Lidar(LIDAR_DEVICE)
# Create an iterator to collect scan data from the RPLidar
self.iterator = self.lidar.iter_scans()
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 update_state(self):
self.current_time = time.time()
self.dt = self.current_time - self.prev_time
self.prev_time = self.current_time
self.omegaRight = self.vr / self.WHEELS_RAD
self.omegaLeft = self.vl / self.WHEELS_RAD
# фактическая линейная скорость центра робота
self.linear_velocity = (self.WHEELS_RAD / 2) * (self.omegaRight +
self.omegaLeft)
#//m/s
# фактическая угловая скорость поворота робота
self.angular_velocity = (self.WHEELS_RAD / self.WHEELS_DIST) * (
self.omegaRight - self.omegaLeft)
self.yaw += (self.angular_velocity * self.dt
) #; # // направление в рад
self.yaw = normalize_angle(self.yaw)
self.x += self.linear_velocity * math.cos(
self.yaw) * self.dt # // в метрах
self.y += self.linear_velocity * math.sin(self.yaw) * self.dt #
def init_lidar(self):
self.lidar = Lidar(LIDAR_DEVICE)
# Create an iterator to collect scan data from the RPLidar
self.iterator = lidar.iter_scans()
# First scan is crap, so ignore it
next(self.iterator)
def stop(self):
# Shut down the lidar connection
print('Stoping.')
file.close()
self.arduino.setSerialData(0, 0)
self.arduino.close_connect()
self.lidar.stop()
self.lidar.disconnect()
def vRToDrive(self, vLinear, vAngular):
return ((2 * vLinear) + (self.WHEELS_DIST * vAngular)) / 2
def vLToDrive(self, vLinear, vAngular):
return ((2 * vLinear) - (self.WHEELS_DIST * vAngular)) / 2
def drive(self, vLinear, vAngular):
vr = self.vRToDrive(vLinear, vAngular)
vl = self.vLToDrive(vLinear, vAngular)
self.arduino.setSerialData(round(vr, 1), round(vl, 1))
def sense(self):
#get odometry data
self.vr, self.vl = self.arduino.getSerialData()
#get laser dara
# Extract (quality, angle, distance) triples from current scan
self.scan = [[item[1], item[2]] for item in next(self.iterator)]
def write_log(self):
dist_vec = scan2distVec(self.scan)
log_data = str(round(self.current_time, 2)) + ' ' + str(
round(self.vr, 2)) + ' ' + str(round(self.vl, 2)) + ' ' + str(
round(self.x, 2)) + ' ' + str(round(self.y, 2)) + ' ' + str(
round(self.yaw, 2)) + ' ' + ' '.join(
str(el) for el in dist_vec) + '\n'
file.write(log_data)
class RPLidar(object):
'''
https://github.com/SkoltechRobotics/rplidar
'''
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 update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
for scan in scans:
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
except serial.serialutil.SerialException:
print(
'serial.serialutil.SerialException from Lidar. common when shutting down.'
)
def run_threaded(self):
sorted_distances = []
if (self.angles != []) and (self.distances != []):
angs = np.copy(self.angles)
dists = np.copy(self.distances)
filter_angs = angs[(angs > self.lower_limit)
& (angs < self.upper_limit)]
filter_dist = dists[(angs > self.lower_limit) &
(angs < self.upper_limit
)] #sorts distances based on angle values
angles_ind = np.argsort(
filter_angs) # returns the indexes that sorts filter_angs
if angles_ind != []:
sorted_distances = np.argsort(
filter_dist) # sorts distances based on angle indexes
return sorted_distances
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
from rplidar import RPLidar
from math import sin, cos, radians
import cv2
lidar = RPLidar('COM3')
data = lidar.iter_scans()
# import json # Use dummy data for no lidar
# with open('data/dummydata.json','r') as f:
# data=json.load(f)
def make_point(bearing, distance):
"""Bearing is the bearing of the lidar reading in degrees, distance is the distance in mm.\nReturns an (x,y) point."""
bearing = radians(bearing)
x = int((sin(bearing) * distance) / 4)
y = int(-1 * ((cos(bearing) * distance)) / 4)
return (x, y)
class Map():
def __init__(self, points=None):
"""The map class is used to hold a set of points on a cartesian plane. It can then be used to perform analysis on these points."""
self.points = points if points else []
def find_edges(self):
"""This function finds the 2 x and 2 y values with the highest frequency of points. These are likely to correspond to the edges of the field."""
# This block is repeated four times in order to find the edge to the left, right, in front and behind the bot.
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()
from rplidar import RPLidar
import numpy as np
LIDAR_PORT_NAME = 'COM14'
lidar = RPLidar(LIDAR_PORT_NAME)
iterator = lidar.iter_scans(max_buf_meas=2000, min_len=0)
def get_value_list():
scan = next(iterator)
offsets = np.array([(np.radians(meas[1]), meas[2]) for meas in scan])
return offsets
class narlam:
def __init__(self):
self.flag = 0
self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS,
'SLAM MAP') # no visualizer needed
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.previous_angles = None
self.x = 0.0
self.y = 0.0
self.theta = 0.0
def slam_no_map(self, path_map_name):
# doing slam with building maps from zero simultaneously
next(self.iterator)
while True:
if self.flag == 1:
break
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
self.slam.update(distances, scan_angles_degrees=angles)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
self.x, local_y, local_theta = self.slam.getpos()
local_theta = local_theta % 360
if local_theta < 0:
self.theta = 360 + local.theta
else:
self.theta = local_theta
self.slam.getmap(self.mapbytes)
# save map generated by slam
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(path_map_name)
self.lidar.stop()
self.lidar.disconnect()
def slam_yes_map(self, path_map_name):
# doing localization only, with pre-built map image file.
with open(path_map_name, "rb") as map_img:
f = map_img.read()
b = bytearray(f)
self.slam.setmap(b)
next(self.iterator)
while True:
if self.flag == 1:
break
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
self.slam.update(distances,
scan_angles_degrees=angles,
should_update_map=False)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles,
should_update_map=False)
self.x, self.y, self.theta = self.slam.getpos()
self.lidar.stop()
self.lidar.disconnect()
class narlam:
def __init__(self):
self.flag = 0
self.pause = 0
self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
#self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.x = 0.0
self.y = 0.0
self.theta = 0.0
def slam_no_map(self, path_map, map_name_pgm, map_name_png):
self.flag = 0
self.pause = 0
path_map_name = path_map + '/' + map_name_pgm # map for reusing
next(self.iterator)
while True:
if self.flag == 1:
break
if self.pause == 1:
continue
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
self.slam.update(distances)
self.previous_distances = distances.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances)
self.x, self.y, local_theta = self.slam.getpos()
local_theta = local_theta % 360
if local_theta < 0:
self.theta = 360 + local.theta
else:
self.theta = local_theta
self.slam.getmap(self.mapbytes)
# On SLAM thread termination, save map image in the map directory
# PNG format(To see/pathplannig)
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(path_map + '/' + map_name_png)
# PGM format(To reuse map)
pgm_save(path_map_name, self.mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
self.lidar.stop()
self.lidar.disconnect()
def slam_yes_map(self, path_map, map_name):
self.flag = 0
self.pause = 0
path_map_name = path_map + '/' + map_name
map_bytearray, map_size = pgm_load(path_map_name)
self.slam.setmap(map_bytearray)
next(self.iterator)
while True:
if self.flag == 1:
break
if self.pause == 1:
pass
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
self.slam.update(distances, should_update_map=False)
self.previous_distances = distances.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
should_update_map=False)
self.x, local_y, local_theta = self.slam.getpos()
self.y = MAP_SIZE_METERS * 1000 - local_y
local_theta = local_theta % 360
if local_theta < 0:
local_theta = 360 + local.theta
else:
local_theta = local_theta
#6/11 -> we found that the vehicle's angle was reversed on the map
self.theta = (local_theta + 180) % 360
self.slam.getmap(self.mapbytes)
self.lidar.stop()
self.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()
init=0
#vmin=1440
#vmax=1470
vmin=1440
vmax=1475
d=5000
window=curses.initscr()
window.nodelay(True)
go=0
try:
for scan in lidar.iter_scans(500,10):
key=window.getch()
data.append(np.array(scan))
X=data[-1]
for j in range(len(X)):
map[min(int(X[j][1])-1,359)]=X[j][2]
mapt=traitement(map)
if key==103:
go=1
if key==115:
curses.endwin()
lidar.stop_motor()
lidar.reset()
lidar.disconnect()
break
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(1000)
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
scan = False
while not scan:
scan = get_next_pts()
lidar = RPLidar('/dev/ttyUSB0')
lidar.clear_input()
enable_kmeans=False
#diagnostics
while(True):
try:
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
scan_FOV=[]
scan_gen= lidar.iter_scans()
#print("Past iter_Scans")
time.sleep(0)
for i, scan in enumerate(scan_gen):
scan_FOV.clear()
n_clusters=10
for cursor in scan:
if((cursor[2]<8000) and((cursor[1]>300 and cursor[1]<361)or((cursor[1]>=0 and cursor[1]<60)))):
scan_FOV.append(cursor)
if (len(scan_FOV)!=0):
if(enable_kmeans):
class hcr():
def __init__(self, ARDUINO_PORT='/dev/ttyACM0', LIDAR_PORT='/dev/ttyUSB0'):
self.connect = self.openconnect(ARDUINO_PORT, ARDUINO_SPEED)
# Connect to Lidar unit
self.lidar = RPLidar(LIDAR_PORT)
# Create an iterator to collect scan data from the RPLidar
self.iterator = self.lidar.iter_scans()
def check_connect(self, connect):
c = connect.read(1).decode()
if c != 'c':
print("false read")
def openconnect(self, port, speed):
connect = serial.Serial(port, speed)
time.sleep(1)
while not connect.is_open:
self.openconnect(port, speed)
is_connected = False
while not is_connected:
print("Waiting for arduino...")
connect.write(start_connect.encode())
connect_flag = connect.read(1).decode()
self.check_connect(connect)
if not connect_flag:
time.sleep(0.1)
continue
if connect_flag == 'r':
is_connected = True
print('Connected!')
return connect
def send(self, lvel, avel):
send_data = set_command + str(round(lvel,2)) + ' ' + str(round(avel,2)) + "\n"
self.connect.write(send_data.encode())
self.check_connect(self.connect)
def lidar_stop(self):
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def arduino_stop(self):
self.setMotors(0,0)
self.connect.close()
def stop(self):
self.lidar_stop()
self.arduino_stop()
def getMotors(self):
self.connect.write(print_command.encode())
data = self.connect.read(12).decode()
#print(data)
self.check_connect(self.connect)
data = data.split(';')
right = float(data[0])
left = float(data[1])
return right, left
def setMotors(self, rightVelocity, leftVelocity):
self.send(rightVelocity, leftVelocity)
#print('2')
#self.check_connect(self.connect)
def getScan(self):
#get laser dara
# Extract (quality, angle, distance) triples from current scan
scan = [[item[1], item[2]] for item in next(self.iterator)]
return scan
def findexactvanes(self, lower_angle_L, lower_angle_R, upper_angle_L,
upper_angle_R, lower_distance_L, lower_distance_R,
upper_distance_L, upper_distance_R):
mylidar = RPLidar("COM3", baudrate=115200)
mylidar_scan = []
totalaverageleftvane = []
totalaveragerightvane = []
totalaverageangleleftvane = []
totalaverageanglerightvane = []
for y in range(0, 20):
for i, scan in enumerate(
mylidar.iter_scans(scan_type='normal', max_buf_meas=60000)
): # scan_type='normal', max_buf_meas=60000
# print('%d: Got %d measures' % (i, len(scan)))
#
mylidar_scan.append(scan)
if i > 10:
break
for i in range(len(mylidar_scan)): # aantal rondes
leftvane = []
rightvane = []
# print("Len lidarscan i : ", len(mylidar_scan[i]))
for j in range(len(
mylidar_scan[i])): # aantal metingen in het rondje
mylist = mylidar_scan[i][j]
if lower_angle_L < mylist[
1] < upper_angle_L and lower_distance_L < mylist[
2] < upper_distance_L:
leftvane.append(mylist)
elif lower_angle_R < mylist[
1] < upper_angle_R and lower_distance_R < mylist[
2] < upper_distance_R:
rightvane.append(mylist)
print("left", leftvane)
print("right", rightvane)
# print("arr_avg: ", arr_avg)
if leftvane:
leftvane = np.array(leftvane)
averageleftvane = np.mean(leftvane[:, 2])
averageangleleftvane = np.mean(leftvane[:, 1])
totalaverageleftvane.append(averageleftvane)
totalaverageangleleftvane.append(averageangleleftvane)
print("Average numpy left", averageleftvane)
if rightvane:
rightvane = np.array(rightvane)
averagerightvane = np.mean(rightvane[:, 2])
averageanglerightvane = np.mean(rightvane[:, 1])
totalaveragerightvane.append(averagerightvane)
totalaverageanglerightvane.append(averageanglerightvane)
print("Average numpy right", averagerightvane)
# mylidar.clean_input()
grandtotalleft = np.mean(totalaverageleftvane)
grandtotalright = np.mean(totalaveragerightvane)
grandtotalleftangle = np.mean(totalaverageangleleftvane)
grandtotalrightangle = np.mean(totalaverageanglerightvane)
print("totaal links:", grandtotalleft)
print("totaal rechts:", grandtotalright)
print("totaal hoek links:", grandtotalleftangle)
print("totaal hoek rechts:", grandtotalrightangle)
mylidar.stop()
mylidar.stop_motor()
mylidar.disconnect()
return grandtotalleft, grandtotalright, grandtotalleftangle, grandtotalrightangle
class LidarGraph(Thread):
def __init__(self, port):
Thread.__init__(self)
self.scan = None
self.running = False
self.port = port
def setup(self):
GPIO.output(23, GPIO.HIGH)
sleep(1)
self.lidar = RPLidar(self.port)
self.lidar.connect()
self.lidar.start_motor()
self.slam = RMHC_SLAM(LaserModel(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=1,
max_search_iter=50)
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
success = False
while success is not True:
try:
self.iterator = self.lidar.iter_scans()
next(self.iterator) #first scan is shit
success = True
except Exception as e:
print('retrying')
print(e)
sleep(0.5)
def restart(self):
self.stop()
self.go()
def go(self):
self.setup()
self.running = True
def stop(self):
print('Stopping')
self.running = False
self.lidar.stop_motor()
self.lidar.disconnect()
del self.lidar
GPIO.output(23, GPIO.LOW) # turn off lidar relay
sleep(1)
def update(self):
self.scan = next(self.iterator)
self.offsets = np.array([(np.radians(meas[1]), meas[2])
for meas in self.scan])
self.intens = np.array([meas[0] for meas in self.scan])
# BreezySLAM
previous_distances = None
previous_angles = None
items = [item for item in self.scan]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
print(str(len(distances)) + ' distances')
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
print('updating slam')
self.slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
self.slam.update(previous_distances,
scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = self.slam.getpos()
print('x ' + str(x) + 'y ' + str(y) + 'theta ' + str(theta))
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
sleep(0.05) #gather more samples?
def data(self):
if self.running != True:
return {'intens': [], 'offsets': []}
return {
'intens': self.intens.tolist(),
'offsets': self.offsets.tolist()
}
def get_map(self):
# slam map
return self.mapbytes
def run(self):
print('run')
iterator = None
while True:
if self.running:
try:
self.update()
print('Updated')
except Exception as e:
print('Exception during update')
print(e)
def slam(currentProgram):
trajectory = []
# Connect to Lidar unit
try:
lidar = Lidar(PORT0)
currentProgram.roombaPort = PORT1
iterator = lidar.iter_scans(1000)
lidar.stop()
next(iterator)
print("Lidar 0, Roomba 1")
except:
print("Roomba 0, Lidar 1")
lidar.stop()
lidar.disconnect()
lidar = Lidar(PORT1)
currentProgram.roombaPort = PORT0
iterator = lidar.iter_scans(1000)
lidar.stop()
next(iterator)
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
previous_distances = None
previous_angles = None
# start time
start_time = time.time()
prevTime = start_time
trigger_start = -100
while (not currentProgram.stop):
SLAMvel = currentProgram.SLAMvals[0]
SLAMrot = currentProgram.SLAMvals[1]
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
l = list(zip(angles,distances))
filtered = list(filter(lambda e: (e[0]>=45 and e[0]<=135) and e[1]<300 , l))
# s = sorted(l, key = lambda e: e[0])
if (len(filtered) > constants.POINTS_THRESHOLD) and (time.time()-trigger_start >5):
currentProgram.programStatus = constants.Status.LIDAR_OBSTACLE
topleft = list(filter(lambda e: (e[0]>=105 and e[0]<=135) , filtered))
front = list(filter(lambda e: (e[0]>=75 and e[0]<=105) , filtered))
topright = list(filter(lambda e: (e[0]>=45 and e[0]<=75) , filtered))
if (len(topleft) > 2):
currentProgram.obstacleLocation[0] = 1
if (len(front) > 2):
currentProgram.obstacleLocation[1] = 1
if (len(topright) > 2):
currentProgram.obstacleLocation[2] = 1
trigger_start = time.time()
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
# print("using speeds ", SLAMvel, SLAMrot)
dt = time.time() - prevTime
slam.update(distances, pose_change=(
(SLAMvel*dt, SLAMrot*dt, dt)), scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
# print("updated - if")
# If not adequate, use previous
elif previous_distances is not None:
# print("using speeds ", SLAMvel, SLAMrot)
dt = time.time() - prevTime
slam.update(previous_distances, pose_change=(
(SLAMvel*dt, SLAMrot*dt, dt)), scan_angles_degrees=previous_angles)
prevTime = time.time()
# print("updated - else")
# Get current robot position
x, y, theta = slam.getpos()
[x_pix, y_pix] = [mm2pix(x), mm2pix(y)]
currentProgram.robot_pos = [
y_pix // constants.CHUNK_SIZE, x_pix // constants.CHUNK_SIZE]
# print("robot_pos - ",x_pix // constants.CHUNK_SIZE,y_pix // constants.CHUNK_SIZE, theta)
# Get current map bytes as grayscale
slam.getmap(currentProgram.mapbytes)
# Shut down the lidar connection
pgm_save('ok.pgm', currentProgram.mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
lidar.stop()
lidar.disconnect()
def slamThread():
global SLAMrot
global SLAMvel
# 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()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
# start time
start_time = time.time()
prevTime = start_time
print("start")
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
print("updated - if")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=previous_angles)
prevTime = time.time()
print("updated - else")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# Get current robot position
x, y, theta = slam.getpos()
# Add new position to trajectory
trajectory.append((x, y))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
if(time.time() - start_time > 30):
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
pgm_save('ok.pgm', mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
exit(0)
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)
def run(lower_angle_L, lower_angle_R, upper_angle_L, upper_angle_R,
lower_distance_L, lower_distance_R, upper_distance_L,
upper_distance_R):
mylidar = RPLidar(PORT_NAME, baudrate=115200)
total = 0
arr_avg_L = []
arr_avg_R = []
avg_angle_L = []
avg_angle_R = []
mylidar_scan = []
angle_L = 0
angle_R = 0
dist_L = 0
dist_R = 0
for y in range(0, 15):
for i, scan in enumerate(
mylidar.iter_scans(scan_type='normal', max_buf_meas=60000)):
# print('%d: Got %d measures' % (i, len(scan)))
#
mylidar_scan.append(scan)
if i > 10:
break
for i in range(len(mylidar_scan)): # aantal rondes
# print("Len lidarscan i : ", len(mylidar_scan[i]))
for j in range(len(
mylidar_scan[i])): # aantal metingen in het rondje
mylist = mylidar_scan[i][j]
# print(mylist[2])
if lower_angle_L < mylist[
1] < upper_angle_L and lower_distance_L < mylist[
2] < upper_distance_L:
dist_L = mylist[2]
angle_L = mylist[1]
total = 1
elif lower_angle_R < mylist[
1] < upper_angle_R and lower_distance_R < mylist[
2] < upper_distance_R:
dist_R = mylist[2]
angle_R = mylist[1]
total = 1
if total == 1:
# aver = som / total
avg_angle_L.append(angle_L)
avg_angle_R.append(angle_R)
arr_avg_L.append(dist_L)
arr_avg_R.append(dist_R)
total = 0
# print("arr_avg: ", arr_avg)
arr_mean_L = np.mean(arr_avg_L)
arr_mean_R = np.mean(arr_avg_R)
arr_avg_angle_L = np.mean(avg_angle_L)
arr_avg_angle_R = np.mean(avg_angle_R)
print("Average Left: ", arr_mean_L)
print("Average Right: ", arr_mean_R)
mylidar.clean_input()
mylidar.stop()
mylidar.stop_motor()
mylidar.disconnect()
return arr_mean_L, arr_mean_R, arr_avg_angle_L, arr_avg_angle_R
print(" ", end=" ")
if check == True:
if num < dConstClose:
x = 2
print("#")
elif num < dConstMedium:
x = 1
print("*")
else:
print(" ")
return x
try:
while (1):
for scan in lidar.iter_scans():
#setting up lists while clearing every iteration
main = []
fList1, fList2, fList3, fList4, fList5, fList6, fList7, fList8, fList9, fList10 = (
[] for i in range(10))
lList1, lList2, lList3, lList4, lList5, lList6, lList7, lList8, lList9, lList10 = (
[] for i in range(10))
rList1, rList2, rList3, rList4, rList5, rList6, rList7, rList8, rList9, rList10 = (
[] for i in range(10))
#setting up average variables while clearing every iteration
fAvg1, fAvg2, fAvg3, fAvg4, fAvg5, fAvg6, fAvg7, fAvg8, fAvg9, fAvg10 = (
0 for i in range(10))
lAvg1, lAvg2, lAvg3, lAvg4, lAvg5, lAvg6, lAvg7, lAvg8, lAvg9, lAvg10 = (
0 for i in range(10))
rAvg1, rAvg2, rAvg3, rAvg4, rAvg5, rAvg6, rAvg7, rAvg8, rAvg9, rAvg10 = (
class RPLidar(object):
'''
https://github.com/SkoltechRobotics/rplidar
'''
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.distances1 = 0
self.angles1 = 0
self.distances2 = 0
self.angles2 = 0
self.distances3 = 0
self.angles3 = 0
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
def update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
d1 = 0
d1_num = 0
a1 = 0
a1_num = 0
d2 = 0
d2_num = 0
a2 = 0
a2_num = 0
d3 = 0
d3_num = 0
a3 = 0
a3_num = 0
for scan in scans:
for index in range(len(scan)):
if scan[index][1] >= 90 and scan[index][1] < 150:
d1 += scan[index][2]
d1_num = d1_num + 1
a1 += scan[index][1]
a1_num = d1_num + 1
elif scan[index][1] >= 150 and scan[index][1] < 210:
d2 += scan[index][2]
d2_num = d1_num + 1
a2 += scan[index][1]
a2_num = d1_num + 1
elif scan[index][1] >= 210 and scan[index][1] < 270:
d3 += scan[index][2]
d3_num = d1_num + 1
a3 += scan[index][1]
a3_num = d1_num + 1
elif (int(scan[index][1])) == 359 and d1_num != 0:
self.distances1 = d1 / d1_num
self.angles1 = a1 / a1_num
self.distances2 = d2 / d2_num
self.angles2 = a2 / a2_num
self.distances3 = d3 / d3_num
self.angles3 = a3 / a3_num
d1 = 0
d1_num = 0
a1 = 0
a1_num = 0
d2 = 0
d2_num = 0
a2 = 0
a2_num = 0
d3 = 0
d3_num = 0
a3 = 0
a3_num = 0
'''
print ("angles: ",self.angles2)
print ("distances: ",self.distances2)
'''
'''
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
'''
except serial.serialutil.SerialException:
print(
'serial.serialutil.SerialException from Lidar. common when shutting down.'
)
def run_threaded(self):
return self.distances1, self.angles1, self.distances2, self.angles2, self.distances3, self.angles3
def run(self):
return self.distances1, self.angles1
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
le plot as rapidly as possible to measure speed.
"""
from PyQt5 import QtGui, QtCore # (the example applies equally well to PySide)
from PyQt5.QtWidgets import QPushButton
import pyqtgraph as pg
import numpy as np
from rplidar import RPLidar
lidar = RPLidar('/dev/tty.SLAB_USBtoUART')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
iterator = lidar.iter_scans(max_buf_meas=2000)
app = QtGui.QApplication([])
mw = QtGui.QMainWindow()
mw.resize(800, 800)
view = pg.GraphicsLayoutWidget(
) ## GraphicsView with GraphicsLayout inserted by default
mw.setCentralWidget(view)
btn2 = QtGui.QPushButton("Stop", view)
lineEdit = QtGui.QLineEdit("", view)
lineEdit1 = QtGui.QLineEdit("", view)
lineEdit2 = QtGui.QLineEdit("", view)
lineEdit3 = QtGui.QLineEdit("", view)
label = QtGui.QLabel("Max quality", view)
label1 = QtGui.QLabel("Error rate", view)
##print("Drone take off...")
## Drone take off - test take of to 10 meters
##droneTakeOff(10, vehicle)
## Wait until start command is send
countZ = 100
radiusAnglesEllipse, ellipsePointsPos, bigSmallRadius = getProperRadii(heightMeasured,largeRArray, smallRArray, angleOffsetMeasured)
if runProgram is True:
print("Starting!")
while attempt < 20 and stopAttempts is not True:
try:
serial_lidar.stop()
for i, scan in enumerate(serial_lidar.iter_scans(1000,5,distance_min_threshold,distance_max_threshold)):
attempt = 0
if len(scan) <= 1:
continue
start_time = timeit.default_timer()
## Angle/Distance numpy array
angleDistanceR = numpy.array(scan)
angleDistanceR[:,0] *= -1
## print(len(angleDistanceR))
## Line filter for filtering the sunlight
## problemIndices = sunFilterV2(angleDistanceR[:,0],angleDistanceR[:,1], distThreshA, distThreshD)
##
def update_line(num, iterator, ax, Aport, lidar, Xs, Ys, Zs, i=[0]):
checkbit = 0
iterbit = 0
checkbit = str(num)
checkbit = checkbit[-1]
checkbit = int(checkbit)
if (checkbit == 0):
iterbit = 0
if (checkbit == 1):
iterbit = 1
if (checkbit == 2):
iterbit = 2
if (checkbit == 3):
iterbit = 3
if (checkbit == 4):
iterbit = 4
if (checkbit == 5):
iterbit = 5
if (checkbit == 6):
iterbit = 6
if (checkbit == 7):
iterbit = 7
if (checkbit == 8):
iterbit = 8
if (checkbit == 9):
iterbit = 9
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
Aport = serial.Serial(strPort, 115200)
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))
angle = (angle) * 0.3515625
# Affine Transform:
angle = np.deg2rad(angle)
# print(angle)
theta = np.array([(np.radians(meas[1])) for meas in scan])
R = np.array([meas[2] for meas in scan])
phi = np.pi / 2
newtheta = []
newR = []
for i in range(len(theta)):
if (theta[i] >= 0) & (theta[i] <= np.pi):
newtheta.append(theta[i])
newR.append(R[i])
newtheta = np.array(newtheta)
newR = np.array(newR)
# Perform Spherical to Cartesian Conversion
X = newR * np.sin(phi) * np.cos(newtheta)
Y = newR * np.sin(phi) * np.sin(newtheta)
Z = newR * np.cos(phi)
Rotate = np.array([[1, 0, 0], [0, np.cos(angle), -np.sin(angle)],
[0, np.sin(angle), np.cos(angle)]])
V = np.array([[X], [Y], [Z]])
Xnew = np.take(Rotate, 0) * X + np.take(Rotate, 1) * Y + np.take(
Rotate, 2) * Z
Ynew = np.take(Rotate, 3) * X + np.take(Rotate, 4) * Y + np.take(
Rotate, 5) * Z
Znew = np.take(Rotate, 6) * X + np.take(Rotate, 7) * Y + np.take(
Rotate, 8) * Z
rows = 2
columns = 500
differenceX = columns - len(Xnew)
storezeroX = np.zeros(shape=(1, differenceX))
appendX = np.append(Xnew, storezeroX)
differenceY = columns - len(Ynew)
storezeroY = np.zeros(shape=(1, differenceY))
appendY = np.append(Ynew, storezeroY)
differenceZ = columns - len(Znew)
storezeroZ = np.zeros(shape=(1, differenceZ))
appendZ = np.append(Znew, storezeroZ)
Xs[iterbit] = appendX
Ys[iterbit] = appendY
Zs[iterbit] = appendZ
print(angle)
ax.clear()
myn = 500
ax.set_zlim(-1 * myn, myn)
ax.set_xlim(-1 * myn, myn)
ax.set_ylim(-1 * myn, myn)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
ax.scatter3D(Xs, Ys, Zs, c='r', s=1)
return Xs, Ys, Zs
def gen():
lidar = RPLidar(PORT_NAME)
return lidar.iter_scans()
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()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
def update_line(num, iterator, ax, Aport, lidar):
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
# Aport = serial.Serial(strPort, 115200)
# 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:
anglecache = []
C = 0
# Test code
while C < 5:
Aport = serial.Serial(strPort, 115200)
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))
angle = (angle) * 0.3515625
anglecache.append(angle)
C = C + 1
print(anglecache)
angle = mode(anglecache)
print(angle)
C = 0
theta = np.array([(np.radians(meas[1])) for meas in scan])
R = np.array([meas[2] for meas in scan])
phi = np.pi / 2
# 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)
Rotate = np.array([[1, 0, 0], [0, np.cos(angle), -np.sin(angle)],
[0, np.sin(angle), np.cos(angle)]])
V = np.array([[X], [Y], [Z]])
Xnew = np.take(Rotate, 0) * X + np.take(Rotate, 1) * Y + np.take(
Rotate, 2) * Z
Ynew = np.take(Rotate, 3) * X + np.take(Rotate, 4) * Y + np.take(
Rotate, 5) * Z
Znew = np.take(Rotate, 6) * X + np.take(Rotate, 7) * Y + np.take(
Rotate, 8) * Z
Vnew = np.array([[Xnew], [Ynew], [Znew]])
offsets = np.array([X, Y, Z])
ax.clear()
ax.set_zlim(-250, 250)
ax.set_xlim(-250, 250)
ax.set_ylim(-250, 250)
ax.scatter3D(Xnew, Ynew, Znew)
return Xnew, Ynew, Znew
class GluttonousSnake:
def __init__(self, state = State.WAIT):
self.state = state
self.running = True
self.car_srl = Car_srlapp()
self.lidar = RPLidar('/dev/ttyUSB0')
# 状态控制
def _state_control(self):
while self.running and self._get_state() is not State.STOP:
dist, _ = self._get_nearest_target()
# 加锁避免状态数据同步冲突
threadLock.acquire()
if self._get_state() is State.WAIT and dist < Wait2HeadThreshold:
self.state = State.HEAD
elif self._get_state() is State.HEAD and dist < Head2FollowThreshold:
self.state = State.FOLLOW
elif self._get_state() is State.WAIT and dist < Follow2StopThreshold:
self.state = State.STOP
threadLock.release()
sleep(StateInspectInterval)
def _wait_main(self):
while self.running and self._get_state() is State.WAIT:
pass
def _head_main(self):
while self.running and self._get_state() is State.HEAD:
dest = self._get_nearest_target()
# 判断是否存在目标,存在则前往，不存在则停止
# 在state_control外进行了状态切换，代码结构上有缺陷，但性能上更好
if dest:
self._go_somewhere(dest)
else:
# 加锁避免状态数据同步冲突
threadLock.acquire()
self.state = State.STOP
threadLock.release()
sleep(HeadPathCorrectionInterval)
def _follow_main(self):
while self.running and self._get_state() is State.FOLLOW:
dest = self._get_nearest_target()
# 考虑前车停止的碰撞问题，对dest进行修正
dest = self._follow_dest_correction(dest)
self._go_somewhere(dest)
sleep(FollowPathCorrectionInterval)
def _stop_main(self):
while self.running and self._get_state() is State.STOP:
self.car_srl.control(MgsType.CONTROL_STOP)
self.running = False
def main(self):
state_control = threading.Thread(target=self._state_control, args=())
wait_main = threading.Thread(target=self._wait_main(), args=())
head_main = threading.Thread(target=self._head_main(), args=())
follow_main = threading.Thread(target=self._follow_main(), args=())
stop_main = threading.Thread(target=self._stop_main(), args=())
threads = {State.HEAD: head_main,
State.FOLLOW: follow_main,
State.STOP: stop_main}
cur_state = self._get_state()
# 如果当前状态为wait，则将wait_main加入threads，便于后续遍历join
if cur_state is State.WAIT:
threads[cur_state] = wait_main
# pre_state用于判断状态切换，以开启新线程
pre_state = cur_state
state_control.start()
threads[pre_state].start()
while self.running:
if pre_state is not self._get_state():
threads[self._get_state()].start()
pre_state = self._get_state()
for thread in threads.values():
thread.join()
state_control.join()
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def _get_state(self):
return self.state
# 雷达数据获取
def _get_radar_signal(self):
scan = next(self.lidar.iter_scans(300, 200))
radar_signal = {}
for i in scan:
radar_signal[i[1]] = i[2]
return radar_signal
# 控制小车朝向dest运动
def _go_somewhere(self, dest):
_, yaw, _ = self.car_srl.request(MgsType.REQUEST_POSE)
# yaw的顺逆时针朝向未知，故此处为+或-待定
yaw_zero = yaw - dest[1]
self.car_srl.control(MgsType.CONTROL_TURN, TurnSpeed)
while abs(yaw_zero - yaw) > AngleDeviationThreshold:
sleep(AngleCorrectionInterval)
_, yaw, _ = self.car_srl.request(MgsType.REQUEST_POSE)
self.car_srl.control(MgsType.CONTROL_STOP)
self.car_srl.control(MgsType.CONTROL_MOVE, MoveSpeed)
# 获取数据梯度信息
def _get_grads(self, sorted_signal):
grads = []
for index in range(1, len(sorted_signal)):
delta = sorted_signal[index - 1][0] - sorted_signal[index][0]
theta = sorted_signal[index - 1][0]
grad = (sorted_signal[index - 1][1] - sorted_signal[index][1]) / delta
grads.append((theta, grad))
return grads
# 根据梯度信息获取所有目标的角度（左右边界角度的平均值）及距离（目前使用的左边界距离）
def _get_targets(self, grads, radar_signal):
targets = []
temp_boundary = 0
for i in grads:
if i[1] < leftGradThreshold and temp_boundary == 0:
temp_boundary = i[0]
elif i[1] > rightGradThreshold and temp_boundary != 0:
targets.append((radar_signal[i[0]], (temp_boundary + i[0])/2))
temp_boundary = 0
return targets
# 返回范围内最近目标的位置信息
def _get_nearest_target(self):
# TODO 跨越360~0边界的目标检测还有BUG存在
radar_signal = self._get_radar_signal()
# 筛选所需范围的雷达数据
radar_signal_ranged = {}
for i in radar_signal:
if scanned_range[self._get_state()][0] <= i <= scanned_range[self._get_state()][1]:
radar_signal_ranged[i] = radar_signal[i]
# 对筛选后的数据进行排序
sorted_signal = sorted(radar_signal_ranged.items(), key=operator.itemgetter(0))
grads = self._get_grads(sorted_signal)
targets = self._get_targets(grads, radar_signal)
# 考虑搜寻不到目标的情况
if targets:
target = min(targets)
else:
target = ()
return target
# TODO 对跟随目标修正，以避免碰撞
def _follow_dest_correction(self, dest):
return dest
class AnimatedScatter(object):
"""An animated scatter plot using matplotlib.animations.FuncAnimation. WhichLidar -> 0 = rplidar, 1 = NONE, 2 = hokuyo
MaxRange, AxisRange - ranges only for visualization
lidarMinThresh, lidarMaxThresh - distance thresholds for what data is taken from the lidar reading
whichCenter - only for demonstration purposes - if 0 then lidar is center of coordinate system, if 1 then the lidar's position is calculated from all it's readings to a arbitrary 0 based coordinate system
"""
def __init__(self,portName = '/dev/ttyACM0', maxRange = 30000, axisRange = 10000, whichLidar = 2, lidarMinThresh = 500, lidarMaxThresh = 5000, whichCenter = 0):
#===============================================================#
#=================== Initialization Block ======================#
#===============================================================#
self.maxRange = maxRange
self.axisRange = axisRange
self.whichLidar = whichLidar
self.portName = portName
self.lidarMinThresh = lidarMinThresh
self.lidarMaxThresh = lidarMaxThresh
self.whichCenter = whichCenter
# global variables
self.anglePCA = 0 # angle of the blade points, calculated using PCA
self.PCACalculated = False # helper bool for determining if blade angle calculated
self.environmentPoints = [] # detected points from blade
self.calculateEllipse = False # is the ellipse calculated
self.ellipseAlgStart = False # is the elliptical algorithm running
self.serial_IMU = serial.Serial("COM10",57600) # IMU + arduino serial, if no IMU is present please comment out
self.arduinoInitial = np.zeros(8)#if no IMU is present please comment out
self.lidarRotAngle = 0 # lidar rotation angle , if no IMU is present please comment out
self.armedAngle = 0 # lidar angle when the blade angle is calculated, used as an initial angle, if no IMU is present please comment out
# initialize lidar - in case of the rpLidar an initial health check is required to be sure that the proper data is sent
if self.whichLidar is 0:
self.lidar = RPLidar(self.portName)
print("Starting Lidar...")
## Start Lidar and get's info
while True:
try:
info = self.lidar.get_health()
break
except RPLidarException:
print("Lidar error retry")
finally:
self.lidar.stop()
print(info)
self.iterator = self.lidar.iter_scans(1000,5,100,6000)
elif self.whichLidar is 1:
pass
# The sweep lidar is removed as it is not currently used
elif self.whichLidar is 2:
self.lidar = lx.connect(self.portName)
lx.startLaser(self.lidar)
#===============================================================#
#========================== BLOCK END ==========================#
#===============================================================#
#===============================================================#
#================== Setup figures and events ===================#
#===============================================================#
# Setup the figure and axes...
self.fig, self.ax = plt.subplots()
self.fig.canvas.mpl_connect('close_event', self.handle_close) # event for clicking the X
self.onClickEv = self.fig.canvas.mpl_connect('button_press_event', self.on_click) # event for clicking the mouse button
# Then setup FuncAnimation. - self.update is the update function, while self.setup_plot is the initialization of figures
self.ani = animation.FuncAnimation(self.fig, self.update, interval=1./40,
init_func=self.setup_plot, blit=True)
#===============================================================#
#========================== BLOCK END ==========================#
#===============================================================#
# Handler function for closing the figure, each lidar has different exit strategies
def handle_close(self,evt):
print('Closed Figure!')
self.fig.canvas.mpl_disconnect(self.onClickEv)
self.serial_IMU.close() # disconnect IMU serial, if no IMU is present please comment out
if self.whichLidar is 0:
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
elif self.whichLidar is 1:
pass
elif self.whichLidar is 2:
self.lidar.close()
# Handler function for mouse click on the canvas
def on_click(self, evt):
# if the blade points orientation is calculated and the elliptical alg is not started - start it now
if self.PCACalculated == True and self.ellipseAlgStart == False:
self.ellipseAlgStart = True
# if the blade points orientation is not calculated - do it
if self.PCACalculated == False:
self.anglePCA = calculateAnglesPCA(self.environmentPoints)
self.PCACalculated = True
print(self.anglePCA)
print("Clicked")
# Setup function for plotting
def setup_plot(self):
"""Setup static markings."""
# Setup axis limits
self.ax.axis([-self.axisRange, self.axisRange, -self.axisRange, self.axisRange])
# Create updating scatter plot and variable for later use
self.scat = self.ax.scatter([], [], c='b', s=1, animated=True, alpha=0.5)
self.scat_lidar = self.ax.scatter([], [], c='r', s=30, animated=True, alpha=0.5)
self.scat_ellipse = self.ax.scatter([], [], c='g', s=20, animated=True, alpha=0.5)
# For FuncAnimation's sake, we need to return the artist we'll be using
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,self.scat_lidar,self.scat_ellipse,
def update(self, i):
"""Update the scatter plot."""
# Update function runs and gets data from the Lidar
if self.whichLidar is 0:
scan = next(self.iterator)
angleDistance = np.array(scan)
elif self.whichLidar is 1:
pass
elif self.whichLidar is 2:
# 0 and 1080 is the degrees that the Hokuyo gets readings from - 0 to 180 degrees, as it checks in 6 increments between a degree
# the last value of 1 signifies if the data will be clustered - if it's 1 data is not averaged by captures
angleDistance, status = lx.getLatestScanSteps(self.lidar, 0, 1080,1)
# Remove data that is farther or closer than the thresholds
angleDistance = angleDistance[np.logical_and(angleDistance[:,1] > self.lidarMinThresh, angleDistance[:,1] < self.lidarMaxThresh)]
# Used only for testing/visualization purposes - when 0 only the environment points are printed and the lidar is always at 0,0
if self.whichCenter == 0:
environmentPoints = circle2cart_points([0,0],angleDistance, 0)
lidarPoint = [0,0]
# Start real algorithm
elif self.whichCenter == 1:
# Get orientation data from IMU, if no IMU present comment out the next three lines
arduinoOutput = getDataFromIMU(self.serial_IMU,self.arduinoInitial)
self.arduinoInitial = arduinoOutput
self.lidarRotAngle = arduinoOutput[4]
# if the elliptical algorithm is started compensate for the lidar's rotation
if self.ellipseAlgStart is True:
compensateYaw = self.lidarRotAngle - self.armedAngle # current rotation angles - the armed angle
angleDistance[:,0] = angleDistance[:,0] + compensateYaw
# Calculate mean angle and mean distance
meanAngle,meanDist = calculateMeans(angleDistance)
# go from polar to carthesian system - position the lidar at a 0,0 coordinate system, then reproject blade points from the lidar's position
lidarPoint = circle2cart_drone([0,0],meanAngle, meanDist)
environmentPoints = circle2cart_points(lidarPoint,angleDistance, 0)
self.environmentPoints = environmentPoints
# is the blade orientation calculated
if self.PCACalculated is True:
if self.calculateEllipse is False:
# Calculate ellipse - first get the major diameter of the ellipse
minDist,maxDist,minDist_points,maxDist_points = calculateMinMax(self.environmentPoints)
# create ellipse using the major diameter, and major diameter/6 as minor diameter, the angle of rotation and a 0,0 center
ellipseRadAngle, self.ellipseBladePoints = testEllipse(int(maxDist/6), int(maxDist), self.anglePCA, [0,0])
self.calculateEllipse = True
# save the armed angle of lidar orientation
self.armedAngle = self.lidarRotAngle
if self.ellipseAlgStart is True:
# get the average detected point position
averagePointPos=[sum(environmentPoints[:,0])/len(environmentPoints[:,0]),sum(environmentPoints[:,1])/len(environmentPoints[:,1])]
pCenter=np.array((0,0))
pAvg = np.array((averagePointPos[0],averagePointPos[1]))
# calculate the distance between the ellipse center and the point
distAveragePointToZero = np.linalg.norm(pCenter-pAvg)
# find the intersection between the ellipse the center of the ellipse and the lidar position
intersectPointOnCurve = intersectionLineCurve([0,0], lidarPoint, self.ellipseBladePoints)
# calculate correction distance - ellipse radius
correctionDist = math.sqrt(intersectPointOnCurve[0]**2 + intersectPointOnCurve[1]**2)
# calculate new center for going from polar to carthesian using the correction distance and the dist of the average point to zero
newCenterPos = circle2cart_drone([0,0],meanAngle, correctionDist -distAveragePointToZero)
lidarPoint = circle2cart_drone(newCenterPos,meanAngle, meanDist)
environmentPoints = circle2cart_points(lidarPoint,angleDistance, 0)
# visualize ellipse
self.scat_ellipse.set_offsets(self.ellipseBladePoints)
# Visualize environment and lidar points
self.scat.set_offsets(environmentPoints)
self.scat_lidar.set_offsets(lidarPoint)
# We need to return the updated artist for FuncAnimation to draw..
# Note that it expects a sequence of artists, thus the trailing comma.
return self.scat,self.scat_lidar,self.scat_ellipse,
def show(self):
plt.show()
