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()
class Lidar(QThread):
# Signal pour envoyer les scans
newScansReceived = pyqtSignal(np.ndarray, np.ndarray, np.ndarray,
np.ndarray)
def __init__(self):
super().__init__()
self.lidar = RPLidar('COM12', baudrate=256000)
#self.lidar = RPLidar('/dev/ttyUSB0', baudrate=256000)
#self.connect()
print('connexion avec le lidar')
self.lidar.logger.setLevel(logging.DEBUG)
consoleHandler = logging.StreamHandler()
#self.lidar.logger.addHandler(consoleHandler)
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
print("####################")
time.sleep(2)
self.continuer = True
def run(self):
gen = self.lidar.scan2ArrayFromLidar(45, 135, 225, 315)
while self.continuer:
radiusRight, thetaRight, radiusLeft, thetaLeft = next(gen)
self.newScansReceived.emit(radiusRight, thetaRight, radiusLeft,
thetaLeft)
print("fin du thread Lidar")
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def 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 connect():
global lidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
lidar.clear_input()
health = lidar.get_health()
print(health)
def run():
lidar = RPLidar(PORT_NAME)
time.sleep(WAIT)
status, code = lidar.get_health()
time.sleep(WAIT)
lidar.disconnect()
print(status)
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()
class Scanner(threading.Thread):
def __init__(self, port):
threading.Thread.__init__(self)
self.lidar = RPLidar(port)
self.lidar.stop_motor()
sleep(0.5)
self.lidar.start_motor()
self.setDaemon(True)
self.scan = [(0.0, 0.0) for i in range(360)]
def check_device(self):
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
return (info, health)
def get_scan(self):
return self.scan
def run(self):
self.lidar.clear_input()
while True:
for meas in self.lidar.iter_measurments():
new_scan = meas[0]
quality = meas[1]
angle = meas[2]
distance = meas[3]
if angle < 360:
self.scan[int(angle)] = (angle, distance / 1000.0)
def 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 main():
lidar = RPLidar(PORT)
# check RPLidar
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
# setup value
points = np.array([[], []])
previous = np.array([[], []])
rotate = IDENTITY
slide = np.array([0, 0])
grid_x = np.linspace(MIN_RANGE, MAX_RANGE, GRID_POINTS)
grid_y = np.linspace(MIN_RANGE, MAX_RANGE, GRID_POINTS)
grid = cal_positive_grid(points, grid_x, grid_y)
fig = plt.figure()
# loop while mapping
for i, scan in enumerate(lidar.iter_scans(min_len=60)):
raw = filter(lambda element: element[1] >= 270 or element[1] <= 90,
scan)
points = transform_raw(raw)
pre, cur = random_filter(previous, points)
if len(pre) > 0 and len(cur) > 0 and i % 5 == 0:
is_change = False
rotation, transform = icp(pre.T, cur.T)
rotation_to_degrees(rotation)
if rotation_to_degrees(rotation) >= 1:
rotate = np.dot(rotation, rotate)
is_change = True
if math.sqrt(transform[0]**2 + transform[1]**2) >= 5:
slide = slide + np.dot(rotate, transform)
is_change = True
if is_change:
previous = points
grid = update_grid(grid,
points.T,
GRID_LENGTH,
grid_x,
grid_y,
origin=slide,
rotation=rotate)
# make occupancy grid
# illustrated
if i % 10 == 0:
occupancy_grid = cal_occupancy_grid(grid)
plt.clf()
plt.pcolormesh(grid_x, grid_y, occupancy_grid)
plt.plot(slide[1], slide[0], 'ro')
tracker = np.dot(rotate, TRACKER)
plt.plot(slide[1] + tracker[1], slide[0] + tracker[0], 'bo')
plt.pause(0.01)
if i == 2:
previous = points
# stop RPlidar
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
# file save
save_override_map(occupancy_grid)
def connect(addr):
lidar = RPLidar(addr)
info = lidar.get_info()
#print(info)
health = lidar.get_health()
#print(health)
return lidar
def connect():
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
return lidar
class RpLidar:
# Constructor
def __init__(self):
# Initialize Lidar Lidar
self.lidar = RPLidar('com3')
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
def runLidar(self):
self.iterator = self.lidar.iter_scans(max_buf_meas=10000)
def stopLidar(self):
self.lidar.stop()
self.lidar.disconnect()
def GetObstacles(self):
scan = next(self.iterator)
obstacles = np.array([[80, 70]])
for y in scan:
# STEP 1: extract polar coordonates from scan
pPolar = [math.radians(y[1]), y[2]] # [angle in radian, distance]
# STEP 2: compute cartesian coordonates
angle = pPolar[0]
passageMatrix = np.array([[np.cos(angle),
np.sin(angle)],
[(np.sin(angle)) * -1,
np.cos(angle)]])
distanceMatrix = np.array([[pPolar[1]], [0]])
pCart = np.dot(passageMatrix, distanceMatrix)
# STEP 4: save the obstacles inside an array
obstacles = np.append(obstacles, [[pCart[0, 0], pCart[1, 0]]],
axis=0)
return obstacles
def ScanEnvironmemnt(self):
cmd = b'\x20' # SCAN_BYTE
self.lidar._send_cmd(cmd)
dsize, is_single, dtype = self.lidar._read_descriptor()
while True:
raw = self.lidar._read_response(dsize)
print(raw)
time.sleep(5)
def main():
flag = False
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
health = lidar.get_health()
print(info, '\n', health)
cap = cv2.VideoCapture(2)
if flag:
roi, mapx, mapy = Calibration(cap)
else:
fs = cv2.FileStorage("callibration.xml", cv2.FILE_STORAGE_READ)
if not (fs.isOpened()):
return 0
roi = fs.getNode('roi').mat()
mapx = fs.getNode('mapx').mat()
mapy = fs.getNode('mapy').mat()
fs.release()
x, y, w, h = roi
hog = cv2.HOGDescriptor()
#svm = ml.SVM_create()
#ml.SVM_load('/HOGlegs70x134.xml')
#hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())
hog.load('/home/vladimir/PycharmProjects/PickToGo/HOGlegs.xml')
#hog.load('/HOGlegs70x134.xml')
while True:
#RunLidar(lidar)
ret, frame = cap.read()
gframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gframe = cv2.remap(gframe, mapx, mapy, cv2.INTER_LINEAR)
gframe = gframe[int(y[0]):int(y[0] + h[0]), int(x[0]):int(x[0] + w[0])]
orig, frame = DetectUsingHOGdetector(hog, gframe)
# upper_bodies = DetectUsingHaar(gframe)
# for (x1, y1, w1, h1) in bodies:
# cv2.rectangle(frame, (x1, y1), (x1+w, y1+h), (0, 255, 0), 2)
# for (x1, y1, w1, h1) in upper_bodies:
# cv2.rectangle(frame, (x1, y1), (x1+w, y1+h), (255, 0, 0), 2)
# for (x1, y1, w1, h1) in lower_bodies:
# cv2.rectangle(frame, (x1, y1), (x1 + w, y1 + h), (0, 0, 255), 2)
cv2.imshow('Detection', frame)
cv2.imshow('Hog', orig)
if cv2.waitKey(1) & 0xFF == ord('q'):
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
break
def lidar_logger(output_directory):
""" LIDAR Logger """
global LIDAR_STATUS, LIDAR_DATA
port_name = '/dev/lidar'
lidar = None
# Configure
config = util.read_config()
while not util.DONE:
try:
lidar = RPLidar(port_name)
print(lidar.get_info())
print(lidar.get_health())
# Open the output file
timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M")
with open(os.path.join(output_directory,timestamp+"_lidar.csv"), "w") as lidar_output:
lidar_output.write("%s %s %s *\n" % (config['time'], "VERSION", json.dumps({"class": "VERSION", "version": util.DATA_API})))
lidar_output.write("%s %s %s *\n" % (config['time'], config['class'], json.dumps(config)))
for _, scan in enumerate(lidar.iter_scans(max_buf_meas=1500)):
lidartime = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ")
data = []
for (_, angle, distance) in scan:
if distance > 0:
data.append((int(angle)%360, int(distance)))
lidar_data = {
'class': 'LIDAR',
'device': 'A1M8',
'time': lidartime,
'scan': data,
}
lidar_output.write("%s %s %s *\n" % (lidar_data['time'], lidar_data['class'], json.dumps(lidar_data)))
LIDAR_DATA = lidar_data
LIDAR_STATUS = True
if util.DONE:
break
except KeyboardInterrupt:
util.DONE = True
except Exception as ex:
print("LIDAR Logger Exception: %s" % ex)
time.sleep(util.ERROR_DELAY)
if lidar is not None:
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
LIDAR_STATUS = False
time.sleep(util.ERROR_DELAY)
def run():
lidar = RPLidar(PORT_NAME)
lidar.stop()
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
print(lidar)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
print(dir(lidar))
def health_check():
lidar = RPLidar(PORT_NAME)
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return
def init():
print('Scanning RPLidar port')
for i in range(20):
try:
port = '/dev/ttyUSB'+str(i)
print('Scanning '+port)
lidar = RPLidar(port)
state = lidar.get_health()
if state[0] == 'Good':
print(state)
return lidar
else:
print(state)
print('1 or more undefined problems ')
except RPLidarException as re:
print(re)
print('Not this one , system continue scanning')
pass
class Lidar:
def __init__(self, port):
self.port = port
self.lidar = RPLidar('/dev/tty.SLAB_USBtoUART')
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
def start(self):
for i, scan in enumerate(self.lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 100:
break
self.stop()
def stop(self):
self.lidar.stop()
#self.lidar.stop_motor()
#self.lidar.disconnect()
from rplidar import RPLidar
lidar = RPLidar('/dev/ttyUSB0')
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
plt.yticks(y, range(-6, 7))
# Add some labels
plt.xlabel("X-Location (meters)")
plt.ylabel("Y-Location (meters)")
ax.add_patch(radius_view)
ax.add_patch(roomba)
if x_y is not None:
ax.plot(x_y[0], x_y[1], 'o')
if __name__ == '__main__':
lidar = RPLidar('/dev/ttyUSB0')
lidar.connect()
print lidar.get_health()
print lidar.get_device_info()
lidar.start_scan()
time.sleep(2) # Let that baby warm up
set_plot()
plt.ion()
plt.show()
try:
while True:
point_list = [p for p in lidar._raw_points]
rp_point_list = [
RPLidarPoint(raw_point) for raw_point in point_list
]
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()
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()
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
import datetime
import time
from rplidar import RPLidar, RPLidarException
adata = []
done = False
data_slice = [0] * 360
stop_at = time.time() + 10
while not done:
try:
lidar = RPLidar("/dev/lidar")
print(lidar.get_info())
print(lidar.get_health())
for i, scan in enumerate(lidar.iter_scans()):
data = []
for _, a, d in scan:
if d > 0:
data.append((round(a) % 360, round(d)))
data_slice[round(a) % 360] = round(d)
obj = {
'type': "LIDAR",
'time':
datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ"),
'scan': data,
}
#print(json.dumps(obj))
if data_slice[90] > 0:
#print(data_slice[89],data_slice[90], data_slice[91])
ax.plot([-1 * (x) for x in x_y[0]], x_y[1], "-" if args.plot_lines else "o")
elif args.front == 3:
ax.plot([-1 * (y) for y in x_y[1]], [-1 * (x) for x in x_y[0]], "-" if args.plot_lines else "o")
elif args.front == 4:
ax.plot(x_y[0], [-1 * (y) for y in x_y[1]], "-" if args.plot_lines else "o")
# else:
# ax.plot(x_y[0], x_y[1], 'o') # This is just for debugging what is wrong with the above TODO
if __name__ == "__main__":
parse_args()
print args.front, type(args.front)
lidar = RPLidar(args.port_name)
lidar.connect()
print lidar.get_health()
print lidar.get_device_info()
lidar.start_scan()
time.sleep(2) # Let that baby warm up
set_plot()
plt.ion()
plt.show()
try:
while True:
point_list = [p for p in lidar._raw_points]
rp_point_list = [RPLidarPoint(raw_point) for raw_point in point_list]
rp_point_list.sort(key=lambda v: v.angle)
_x = []
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()
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 connect(self):
lidar = RPLidar(self.addr) # demarre le lidar
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
serial_IMU = serial.Serial(portDictionary['3'],57600)
arduinoInitial = numpy.zeros(8)
print("Importing radii...")
## Import radii and correction parameters
largeRArray, smallRArray = radiiImport()
distCorrLidar = errorCorrectionImport()
print("Starting Lidar...")
## Start Lidar and get's info
while True:
try:
info = serial_lidar.get_health()
break
except RPLidarException:
print("Lidar error retry")
finally:
serial_lidar.stop()
print(info)
##print("Starting IMU calibration...")
#### Calibrate IMU
##try:
