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()