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 stop(port): lidar = RPLidar(port) lidar.start_motor() sleep(0.01) lidar.stop() lidar.stop_motor() lidar.disconnect()
def init(): lidar = RPLidar('/dev/ttyUSB0') plt.plot(0, 0, 'r*') while (1): lidar.connect() lidar.start_motor() time.sleep(1) #begins the sampling of data and places it into scan list try: for scan in lidar.iter_scans(): for quality, angle, distance in scan: x_pos = (math.cos(deg_to_rad(angle)) * distance) * 0.001 #Conversion y_pos = (math.sin(deg_to_rad(angle)) * distance) * 0.001 #Conversion plt.scatter(x_pos, y_pos, color='green') plt.pause(0.5) except Exception as e: pass else: print(e) lidar.stop() lidar.stop_motor() lidar.disconnect() break lidar.stop() lidar.stop_motor() lidar.disconnect()
def run(path): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.disconnect() lidar.connect() lidar.start_motor() lidar.stop_motor() lidar.start_motor() outfile = open(path, 'a') base = time.time() tiempo_pasado = 0 try: print('Recording measurments... Press Crl+C to stop.') for measurment in lidar.iter_measurments(): line = '\t'.join(str(v) for v in measurment) tiempo_pasado = time.time() - base line = line + '\t' + 'time\t' + str(tiempo_pasado) outfile.write(line + '\n') except KeyboardInterrupt: print('Stoping.') lidar.stop_motor() lidar.stop() lidar.disconnect() outfile.close()
class Wrapper(object): # wrapper object def __init__(self, port): # set up LiDAR connection self.lidar = RPLidar(port) self.lidar.stop_motor() # stop motor def start(self): # start motor self.lidar.start_motor() def stop(self): # stops motor self.lidar.stop_motor() def output(self, amount): # samples for number of seconds i = 0 measurements = [] # stores readings secs = amount # 2000 samples per second begin = time.time() # grab unix start time for measurement in self.lidar.iter_measurments( ): # iterate through measurements measurements.append(measurement) # add to measurement array i = i + 1 if i == secs: # break if done sampling end = time.time() # grab unix end times break print('Begin: ', begin) # print start times, end times, and time elapsed print('End: ', end) print('Elapsed: ', end - begin) self.lidar.stop_motor() # stop motor self.lidar.stop() # stop scanning return measurements
def run(): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) try: print('Recording measurments... Press Crl+C to stop.') try: for measurment in lidar.iter_measurments(): # line = '\t'.join(str(v) for v in measurment) # print(line + '\n') if (measurment[2] > 0 and measurment[2] < 90): if (measurment[3] < 1000 and measurment[3] > 100): print("Angle: ", measurment[2] - angleoffset) print("Distance: ", measurment[3]) steer(measurment[2] - angleoffset) except KeyboardInterrupt: print('Stopping.') except KeyboardInterrupt: print('Stopping.') lidar.stop() lidar.stop_motor() c.send("motors", 0, 0, 0) # turn off wheel motors lidar.disconnect()
def run(MOTOR_PWM): lidar = RPLidar(PORT_NAME) time.sleep(WAIT) lidar.reset() time.sleep(WAIT) lidar._motor_speed = MOTOR_PWM time.sleep(WAIT) lidar.start_motor() time.sleep(WAIT) lidar.disconnect()
def run(): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) try: scan(lidar) except KeyboardInterrupt: stop = True print('Stopping.') lidar.stop() lidar.stop_motor() lidar.disconnect()
def detect(lidar_ranges, port): lidar = RPLidar(port) lidar.start_motor() sleep(1) COUNT_LIMIT = 10 dispatcher = FilterDispatcher(lidar_ranges, COUNT_LIMIT) try: for _, _, angle, distance in lidar.iter_measures(scan_type='normal', max_buf_meas=4500): process_data(dispatcher, distance, angle) except RPLidarException as e: print(e) finally: lidar.stop() lidar.stop_motor() lidar.disconnect()
class Rplidar(Component): def __init__(self, cfg): super().__init__(inputs=[], outputs=['lidar', ], threaded=True) self.port = cfg['lidar'] self.distance = [] # List distance, will be placed in datastorage self.angles = [] # List angles, will be placed in datastorage self.lidar = RPLidar(self.port) self.on = True self.lidar.clear_input() def onStart(self): """Called right before the main loop begins""" self.lidar.connect() self.lidar.start_motor() def step(self, *args): return self.distance, self.angles, def thread_step(self): """The component's behavior in its own thread""" scans = self.lidar.iter_scans() while self.on: try: for distance, angle in scans: for item in distance: # Just pick either distance or angle since they have the same amount in list self.distance = [item[2]] # Want to get the 3rd item which gives the distance from scans self.angles = [item[1]] # Want to get the 2nd item which gives the angle from scans except serial.SerialException: print('Common exception when trying to thread and shutdown') def onShutdown(self): """Shutdown""" self.on = False self.lidar.stop() self.lidar.stop_motor() self.lidar.disconnect() def getName(self): return 'Rplidar'
def run(): '''Main function''' global stop if stop == False: lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) ioloop = asyncio.get_event_loop() tasks = [ioloop.create_task(scan(lidar)), ioloop.create_task(drive())] try: ioloop.run_until_complete(asyncio.wait(tasks)) except KeyboardInterrupt: print('Stopping.') stop = True lidar.stop() lidar.stop_motor() c.send("motors", 0, 0, 0) # turn off wheel motors lidar.disconnect() ioloop.close()
class Lidar(Thread): def __init__(self, port, baud, data, limit=500): Thread.__init__(self) self.lidar = RPLidar(port=port, baudrate=baud) try: print(self.lidar.get_info()) except Exception as e: print(e) self.lidar.eteindre() self.lidar.reset() self.lidar.connect() self.lidar.start_motor() self.data = data self.limit = limit def run(self): sleep(1) try: for measurment in self.lidar.iter_measurments( max_buf_meas=self.limit): s = len(self.data[0]) if s >= self.limit: self.data[0].pop(0) self.data[1].pop(0) self.data[0].append(measurment[2] * pi / 180) #agle en rad self.data[1].append(measurment[3] / 1000) #distance en m except Exception as error: print(error) def join(self, timeout=None): self.eteindre() Thread.join(self, timeout) def eteindre(self): self.lidar.stop() self.lidar.stop_motor() self.lidar.disconnect()
def run(): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) drive(0) # start going forward lasttime = int(time.time() * 1000) try: counter = 0 print('Recording measurements... Press Crl+C to stop.') data = 0 try: for measurment in lidar.iter_measurments(): if ((int(time.time() * 1000) - lasttime) < 1000): raise ValueError # do this loop at 10Hz if (measurment[2] > 0 and measurment[2] < 90): # in angular range if (measurment[3] < 1000 and measurment[3] > 100): # in distance range data = data + measurment[2] # angle counter = counter + 1 # increment counter except ValueError: print("this should happen ten times a second") lasttime = int(time.time() * 1000) # reset 10Hz timer if counter > 0: # this means we see something average_angle = data / counter print("Average Angle: ", average_angle - angleoffset) drive(average_angle - angleoffset) except KeyboardInterrupt: print('Stopping.') lidar.stop() lidar.stop_motor() c.send("motors", 0, 0, 0) # turn off wheel motors lidar.disconnect()
def __init__(self): super().__init__("teleop_controller") args= sys.argv self.name = "teleop_default_name" self.save_index = 0 if "--name" in args: self.name = args[args.index("--name") + 1] print("Teleop Running: ") print("Type(w,a,s,d,x) to move ") print("Type 'q' to quit the node") print("Type 'k' to activate image detection") print("Type 'l' to de-activate image detection") print("Type 'b' to start the Motors") print("Type 'n' to stop the Motors") print("Type 'f' to flip the camera") print("Type '5' to change the map quality") print("Type '6' to draw a line in the controller 1 logs") print("Type '7' to take a picture now") print("--name = ", self.name) self.uart_publisher = self.create_publisher(String, 'uart_commands', 1000) self.cam_control_publisher = self.create_publisher(String, 'detectnet/camera_control', 1000) self.camera_flip_topic = self.create_publisher(String, 'video_source/flip_topic', 1000) self.map_corner_client = self.create_client(FindMapCorner, "find_map_corner") self.map_quality_control = self.create_publisher(String, 'slam_control', 1000) self.log_line_printer = self.create_publisher(String, 'log_line', 5) self.flip = True try: while(1): key = self.getKey() if key == 'l': # stop detection self.cam_control_publisher.publish(String(data="destroy")) elif key == 'k': # activate detection self.cam_control_publisher.publish(String(data="create")) elif key == 'b': # start the motor lidar = Lidar('/dev/ttyUSB0') lidar.start_motor() elif key == 'n': # stop the motor lidar = Lidar('/dev/ttyUSB0') lidar.stop_motor() elif key == 'f': # flip the camera msg = "normal" if self.flip else "flip" self.flip = not self.flip self.camera_flip_topic.publish(String(data=msg)) elif key == '5': # change map quality self.map_quality_control.publish(String(data="nimportequoi")) elif key == '6': self.log_line_printer.publish(String(data="pinguing")) elif key == '7': self.send_service() elif key == '0': break else: msg = String() msg.data = key self.uart_publisher.publish(msg) raise RuntimeError("Teleoperation was aborted") except: print(e) finally: termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
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()
)**2 #Cam - determines how closely the lines follow each other- Simulation Purposes Only Rsim = np.diag( [1.0, np.deg2rad(30.0)] )**2 # Cam - Changes the correlation between dead reckoning and actual posiitoning DT = 0.1 # time tick [s] #Cam - how many times it updates over the course of the simulation, but also changes the simulation time by a predictable about (multiply DT by 9 = divide sim time by 9) SIM_TIME = 50.0 # simulation time [s] # Cam - how many "seconds", partially relative to DT (see comment above), in the simulation MAX_RANGE = 20.0 # maximum observation range # Cam - maximum domain and range of the simulation coordinate plane # Particle filter parameter NP = 100 # Number of Particle # Cam - exactly what they said NTh = NP / 2.0 # Number of particle for re-sampling # Cam - how many points seem reasonable to the program show_animation = True #Cam - determines if the animation will be shown lidar.start_motor() #lidar.connect() def scan(path): '''Main function''' for measurment in lidar.iter_measurments(): print(measurment[2]) def calc_input(): print("running calc") #print(lidar.measurment[0]) v = 1.0 # [m/s] #Cam - velocity of the simulated robot yawrate = 0.1 # [rad/s] #Cam- rotational rate of the robot u = np.array([[
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)