def __init__(self):
self.lidar_scanning = True
self.initial_scanning_port_num = 0
###############
logging.basicConfig(filename='LiDAR_main.log',filemode = 'a',level =logging.INFO)
logging.info('Scanning RPLidar port')
###############
while self.lidar_scanning:
try:
self.lidar_USB_port = '/dev/ttyUSB'+str(self.initial_scanning_port_num)
#############
logging.info('Scanning '+self.lidar_USB_port)
#############
self.lidar = rplidar.RPLidar(self.lidar_USB_port)
self.lidar_state = self.lidar.get_health()
if self.lidar_state[0] == 'Good':
logging.info(self.lidar_state)
self.lidar_scanning = False
else:
print(self.lidar_state)
print('1 or more undefined problems ')
except rplidar.RPLidarException:
# print(re)
logging.info('Not this one , system continue scanning')
def __init__(self, PORT):
while 1:
try:
self.lidar = rp.RPLidar(PORT, 115200)
self.PORT = PORT
break
except:
pass
def connect_lidar(self):
try:
self.lidar = rplidar.RPLidar(self.lidar_port)
except rplidar.RPLidarException as e:
print('LIDAR COULDNT CONNECT TO ', self.lidar_port, e)
return
self.connected = self.connected | ConnectionStates.LIDAR_CONNECTED
self.lidar_thread = Thread(target=lidar_loop,
args=(self.lidar, self)) # create thread
self.lidar_thread.start()
def __init__(self):
"""
Inicialización de la clase para escaneo con
le LiDAR
Parametros
----------
Ninguno
"""
#Inicializacion Servo en inicial
self.servo = Servo(90)
#Inicializacion Lidar (en puerto ttyUSB0 o ttyUSB1)
if exists("/dev/ttyUSB0"):
self.lidar = rplidar.RPLidar("/dev/ttyUSB0")
else:
self.lidar = rplidar.RPLidar("/dev/ttyUSB1")
sleep(1)
self.lidar.get_info() #Informacion del Lidar
self.lidar.get_health() #Informacion del estado del Lidar
self.process_scan = lambda scan: None #Lectura de generadores
def get_lidar_data(lidar_data, get_lidar_data_run):
lidar = rplidar.RPLidar("COM12")
time.sleep(1)
run = True
# print(lidar.get_info())
# print(run)
while run:
try:
for data in lidar.iter_scans(300):
# print(data)
# make sure lidar_data queue is always the lastest lidar data
# if there is data left, take out the rest. If no data left,
# just input lidar data.
if lidar_data.qsize() > 0:
# print("size > 0")
lidar_data.get()
lidar_data.put(data)
else:
lidar_data.put(data)
# Check if queue, get_lidar_data_run has an input of Flase.
try:
run = get_lidar_data_run.get(False)
if not run:
lidar.stop()
print("Lidar stop")
break
except queue.Empty:
pass
except KeyboardInterrupt:
lidar.stop()
print("Lidar stopped by KeyboardInterrupt")
except:
# print("OMG")
traceback.print_exc()
lidar.stop()
finally:
run = False
lidar.disconnect()
print("Lidar disconnect")
def run(Number_of_turns=1):
data=[]
lidar = rp.RPLidar(PORT_NAME)
lidar.clear_input()
info = lidar.get_info()
data.append(info)
health = lidar.get_health()
data.append(health)
for i,scan in enumerate(lidar.iter_measurments()):
data.append(scan)
if i>Number_of_turns-2:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return data
def lidar_scan_port(self):
retry = 0
self.lidar_scanning_flag = True
self.initial_scanning_port_num = 0
logging.info('Scanning RPLidar port')
while self.lidar_scanning_flag:
try:
self.lidar_USB_port = '/dev/ttyUSB'+str(self.initial_scanning_port_num)
logging.info('Scanning '+self.lidar_USB_port)
self.lidar = rplidar.RPLidar(self.lidar_USB_port)
time.sleep(0.1)
self.lidar_state = self.lidar.get_health()
if self.lidar_state[0] == 'Good':
logging.info(self.lidar_state)
self.lidar_scanning_flag = False
self.lidar_connect = True
logging.info("lidar initialize successuflly")
else:
print(self.lidar_state)
print('1 or more undefined problems , plase check RPLiDAR')
logging.warning(str(self.lidar_state)+' ; 1 or more undefined problems , please check RPLiDAR')
except rplidar.RPLidarException:
# print("Not this one , system continue scanning")
logging.info('Not this one , system continue scanning')
self.initial_scanning_port_num += 1
if retry < 5:
if self.initial_scanning_port_num > 5:
self.initial_scanning_port_num = 0
retry += 1
logging.warning('Rescanning RPLiDAR port')
else:
self.lidar_scanning_flag = False
except:
traceback.print_exc()
logging.exception("Got error\n")
async def _get_container(simulation: bool, stub_lidar: bool,
stub_socket_can: bool) -> DependencyContainer:
"""
Build the dependency container.
"""
i = DependencyContainer()
i.provide('configuration', CONFIG)
i.provide('protobuf_handler', ProtobufHandler)
i.provide('odometry_controller', OdometryController)
i.provide('localization_controller', LocalizationController)
i.provide('motion_controller', MotionController)
i.provide('strategy_controller', StrategyController)
i.provide('symmetry_controller', SymmetryController)
i.provide('lidar_controller', LidarController)
i.provide('debug_controller', DebugController)
i.provide('match_action_controller', MatchActionController)
i.provide('motion_gateway', MotionGateway)
i.provide('simulation_probe', SimulationProbe)
i.provide('event_loop', asyncio.get_event_loop())
if simulation:
i.provide('simulation_configuration', SIMULATION_CONFIG)
i.provide('event_queue', EventQueue())
i.provide('simulation_handler', SimulationHandler)
i.provide('simulation_runner', SimulationRunner)
i.provide(
'simulation_state',
SimulationState(time=0,
cups=[],
left_tick=0,
right_tick=0,
last_position_update=0,
last_lidar_update=0))
i.provide('simulation_gateway', SimulationGateway)
i.provide('http_client', HTTPClient)
i.provide('web_browser_client', WebBrowserClient)
i.provide('replay_saver', ReplaySaver)
if simulation or stub_lidar:
i.provide('lidar_adapter', SimulatedLIDARAdapter)
else:
rplidar_obj = rplidar.RPLidar(list_ports.comports()[0].device)
i.provide('rplidar_object', rplidar_obj)
i.provide('lidar_adapter', RPLIDARAdapter)
if simulation or stub_socket_can:
i.provide('socket_adapter', LoopbackSocketAdapter)
i.provide('motor_board_adapter', LoopbackSocketAdapter)
else:
reader, writer = await tcp.get_reader_writer('localhost', 32000)
i.provide('motor_board_adapter',
TCPSocketAdapter,
reader=reader,
writer=writer)
return i
def reconnect(self):
self.lidar.stop()
self.lidar = rplidar.RPLidar(self.lidar_USB_port)
import rplidar
import numpy as np
import cv2
import math
lidar = rplidar.RPLidar('/dev/cu.SLAB_USBtoUART')
info = lidar.get_info()
print(info)
lidar.clear_input()
health = lidar.get_health()
print(health)
angles = np.zeros(361)
distances = np.zeros(361)
start_point = (200, 200)
image = np.zeros((400, 400, 3), np.uint8)
# while True:
for i, angle in enumerate(lidar.iter_measurments()):
angles[int(angle[2])] = angle[2]
distances[int(angle[2])] = angle[3]
if i > 2000:
break
for i in range(distances.size):
dist = distances[i] / 1000 if distances[i] != 0 else 1
def __init__(self, PORT):
self.lidar = rp.RPLidar(PORT, 115200)
'''
#!/usr/bin/env python3
import time
import signal
from UDPComms import Publisher
import rplidar
pub = Publisher(8110)
use_express = False
possible_ports = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/tty.SLAB_USBtoUART']
for port in possible_ports:
try:
lidar = rplidar.RPLidar(port)
break
except rplidar.RPLidarException:
pass
else:
raise rplidar.RPLidarException(
"Can't find serial port to connect to sensor")
def signal_term_handler(signal, frame):
lidar.stop()
lidar.stop_motor()
exit()
signal.signal(signal.SIGTERM, signal_term_handler)
import rplidar
import i2c_bus
print("Initializing I2C bus object")
i2c = i2c_bus.I2C_bus()
print("Creating lidar object")
lidar = rplidar.RPLidar()
lidar.reset()
i2c.setLidarMemoryViews(lidar.get_headings_mv(), lidar.get_distances_mv())
i2c.sendLidarReadings()
lidar.start_scanning()
while True:
lidar.update()
i2c.update(lidar)
import rplidar
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1
import sys
import numpy as np
import atexit
import serial
import time
if __name__ == '__main__':
lidar = rplidar.RPLidar(sys.argv[1])
print('RPLidar health:', lidar.get_health()[0])
print('RPLidar info: ', lidar.get_info())
lidar.start_motor()
scans_iterator = lidar.iter_scans()
slam = RMHC_SLAM(RPLidarA1(), 800, 5)
uvon_dev = serial.Serial(sys.argv[2], 115200)
time.sleep(2)
uvon_dev.write(b'2\n')
uvon_dev.write(b'I1\n')
atexit.register(lambda: uvon_dev.write(b'2\n'))
async def _get_container(simulation: bool, stub_lidar: bool,
stub_socket_can: bool) -> DependencyContainer:
"""
Build the dependency container.
"""
i = DependencyContainer()
i.provide('configuration', CONFIG)
i.provide('protobuf_router', ProtobufRouter)
i.provide('odometry_function', lambda: odometry_arc)
i.provide('position_controller', PositionController)
i.provide('motor_gateway', MotorGateway)
i.provide('motion_controller', MotionController)
i.provide('trajectory_controller', TrajectoryController)
i.provide('strategy_controller', StrategyController)
i.provide('symmetry_controller', SymmetryController)
i.provide('obstacle_controller', ObstacleController)
i.provide('debug_controller', DebugController)
i.provide('match_action_controller', MatchActionController)
i.provide('probe', Probe)
i.provide('event_loop', asyncio.get_event_loop())
i.provide('http_client', HTTPClient)
i.provide('web_browser_client', WebBrowserClient)
i.provide('replay_saver', ReplaySaver)
if simulation:
i.provide('simulation_configuration', SIMULATION_CONFIG)
i.provide('simulation_router', SimulationRouter)
i.provide('simulation_runner', SimulationRunner)
i.provide(
'simulation_state',
SimulationState(time=0,
cups=[],
left_tick=0,
right_tick=0,
left_speed=0,
right_speed=0,
queue_speed_left=deque(),
queue_speed_right=deque(),
last_position_update=0,
last_lidar_update=0))
i.provide('simulation_gateway', SimulationGateway)
i.provide('clock', FakeClock)
else:
i.provide('clock', RealClock)
if simulation or stub_lidar:
i.provide('lidar_adapter', SimulatedLIDARAdapter)
else:
rplidar_obj = rplidar.RPLidar(list_ports.comports()[0].device)
i.provide('rplidar_object', rplidar_obj)
i.provide('lidar_adapter', RPLIDARAdapter)
servo_adapter_list: List[SocketAdapter] = []
if simulation or stub_socket_can:
i.provide('motor_board_adapter', LoopbackSocketAdapter)
for _ in range(NB_SERVO_BOARDS):
servo_adapter_list.append(LoopbackSocketAdapter())
else:
i.provide('motor_board_adapter',
ISOTPSocketAdapter,
address=NET_ADDRESS_MOTOR,
adapter_name='motor_board')
for index in range(NB_SERVO_BOARDS):
servo_adapter_list.append(
ISOTPSocketAdapter(address=NET_ADDRESSES_SERVO[index],
adapter_name="servo_board_" + str(index)))
i.provide('servo_adapters_list', servo_adapter_list)
i.provide('actuator_gateway', ActuatorGateway)
i.provide('actuator_controller', ActuatorController)
return i
lidarUnit.stop_motor()
exit()
currentLoopCounter = 0
# Connects to microcontroller and lidar
portList = list_ports.comports()
MCUUnit = None
lidarUnit = None
print("Seeking Peripherals")
print(portList)
#for currentPortIndex in range(0, len(portList)):
#currentPort = portList[currentPortIndex]
# May be Lidar
try:
lidarUnit = rplidar.RPLidar('/dev/ttyUSB0')
lidarUnit.stop()
lidarUnit.stop_motor()
lidarUnit.clean_input() # :(
lidarUnit.start_motor()
time.sleep(5) # Lidar likes to freak out if not already spinning
signal.signal(signal.SIGTERM, signal_term_handler)
except rplidar.RPLidarException as e:
lidarUnit = None
print(e)
# May be MCU
try:
serialPort = serial.Serial('/dev/ttyACM0', timeout=0.1)
serialPort.close()
serialPort.open()
def recorte():
lista = []
for a in range(len(scan)):
if scan[a,1]<150:
lista.append(a)
aux = scan[lista]
return aux
data = np.array([[0],[0],[0]]) #Vector columna inicio
def empaquetamiento_cartesinano():
aux = np.concatenate(([x],[y],[z]),axis=0) #Concatena a = x,y,z
aux = np.concatenate((data,aux),axis=1) #Concatena concatena planos
return aux
#%% ------------------- INICIALIZACION LIDAR & SERVO ------------------
lidar = rplidar.RPLidar(PuertoUSB)
servo = Servo(AInit)
sleep(1)
info =lidar.get_info()
print(info)
lidar.get_health()
# plt.figure(1)
# plt.scatter(0,0,cmap="red")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(0, 0, 0, marker='o');
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
#ax.set_ylim(-1,300)
movement_step_for_live = 0
current_movement = 0
movement_step_pickled = 0
movement_step_for_file = 0
scan_number = 1
stop = True
saved = True
x_for_live = []
x_for_file = []
distance_for_live = []
distance_for_file = []
movement_for_live = []
movement_for_file = []
raw_scan_data = [0] * 360
# csv_file = open("lidar_data.csv", "w")
try:
print("[~] Connecting with LiDAR device on port: " + str(PORT) + " ...")
lidar = rplidar.RPLidar(PORT)
print("[+] Successfully connected with LiDAR device")
except Exception as e:
print("[-] An error has occurred while trying to connect with LiDAR device:", str(e))
print_exc()
exit()
lidar_handler()
#%%
import rplidar as rplidar
import serial
from time import sleep,time
import numpy as np
import matplotlib.pyplot as plt
#%%
lidar = rplidar.RPLidar('COM4')
sleep(1)
info =lidar.get_info()
print(info)
lidar.get_health()
# plt.figure(1)
# plt.scatter(0,0,cmap="red")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(0, 0, 0, marker='.');
plt.grid(True)
process_scan = lambda scan: None
#Angulos en Z
#a=np.arange(95,80,-5)
a=np.array([90,90,90])
b=['Orig']
for i in a: b.append(str(i))
phi=a*np.pi/180 #90 a 60
print(phi)
print(b)
def recorte():
lista = []
