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 = []