def __enter__(self): self._running = True # Start sweep thread self.sweep = Sweep(self.DEV).__enter__() self.sweep.start_scanning() self.st.start() # Start Sweep thread # Data server: self.ds, self.dst = dataserver.start( ) # Start data server, and store socket server and thread objects return self
def main(): """Creates a 3D scanner and gather a scan""" #Initialize Node and handles rospy.init_node('sweep_node_motion', anonymous=True) exporter = scan_exporter_f.ScanExporter() # Create a scanner base #base = scanner_base.ScannerBase() #settings for sweep settings = scan_settings.ScanSettings() # Create sweep sensor, and perform scan with Sweep('/dev/ttyUSB0') as sweep: # Create a scanner object time.sleep(1.0) scanner = Scanner(device=sweep, settings=settings, exporter=exporter) # Setup the scanner scanner.setup() # Perform the scan scanner.perform_scan() while not rospy.is_shutdown(): rospy.spin()
def main(arg_dict): """Creates a 3D scanner and gather a scan""" # Create a scan settings obj settings = scan_settings.ScanSettings( int(arg_dict['motor_speed']), # desired motor speed setting int(arg_dict['sample_rate']), # desired sample rate setting int(arg_dict['dead_zone']), # starting angle of deadzone int(arg_dict['angular_range']), # angular range of scan # mount angle of device relative to horizontal int(arg_dict['mount_angle'])) # Create an exporter exporter = scan_exporter.ScanExporter(file_name=arg_dict['output']) with Sweep('/dev/ttyUSB0') as sweep: # Create a scanner object time.sleep(1.0) scanner = Scanner(device=sweep, settings=settings, exporter=exporter) # Setup the scanner scanner.setup() # Perform the scan scanner.perform_scan() # Stop the scanner time.sleep(1.0) scanner.idle()
def useSweep(comPort, motorSpeed, sampleRate): # Connect to the Sweep Lidar print('Connecting to the Sweep sensor on COM port \"{}\"'.format(comPort)) with Sweep(comPort) as sweep: print('Connected to the Sweep sensor\n') print('Changing settings to\nMotor Speed\t{} Hz\nSample Rate\{} Hz'. format(motorSpeed, sampleRate)) ss.changeSettings(sweep, motorSpeed, sampleRate) print('Settings injected') print('Starting scanning') sweep.start_scanning() # Infinit loop, the loop i executed everytime a new scan is present from the LIDAR try: for scan in sweep.get_scans(): lidarSamples = ss.scanToNpArray(scan) lidatSamplesFiltered, lidarSamplesBad = ss.filterSignalStrength( lidarSamples, signalStrengthValue) lidatSamplesFiltered, _ = ss.filterDistance( lidatSamplesFiltered, distanceMin, distanceMax) lidarSamplesBad = np.concatenate((lidarSamplesBad, _)) print('{}\\{}\\{}'.format(len(lidarSamples), len(lidatSamplesFiltered), len(lidarSamplesBad))) getInputAndVisualize(lidatSamplesFiltered[:, :2], _) except KeyboardInterrupt: print("exit") plt.close()
def run(self): """ Start scanner thread. Puts the scans into a queue until the `stop_criterion` is achieved The scan is put in a dict alongside a timestamp """ with Sweep(self.dev) as sweep: sweep.start_scanning() try: for scan in sweep.get_scans(): if self.stop_criterion.is_set(): sweep.stop_scanning() print(sweep.get_sample_rate()) self.queue.put_nowait(None) sweep.set_motor_speed(0) # turn off scanner print("gathered {} scans".format(self.counter)) break else: self.queue.put_nowait({ 'time': datetime.datetime.now().timestamp(), 'scan': scan }) self.counter += 1 except RuntimeError as e: print(e) self.queue.put_nowait(None) sweep.stop_scanning() sweep.set_motor_speed(0) # turn off scanner print("gathered {} scans".format(self.counter))
def scanning(self): with Sweep('/dev/ttyUSB0') as sweep: sweep.start_scanning() for scan in sweep.get_scans(): data = ('{}\n'.format(scan)) self.current_scan = process_scans(data) self.current_scan_time = time.asctime()
def handle_scanning_poses_request(req): rospy.loginfo('PoseSettingRequest Received array length %s', len(req.path_x)) if len(req.path_x) == epochs: success = True for i in range(0, epochs): print "epochs = %d" %(i+1) global Pose_x Pose_x = req.path_x[i] global Pose_y Pose_y = req.path_y[i] global Pose_z Pose_z = req.path_z[i] print("pose x = %f") %Pose_x print("pose y = %f") %Pose_y print("pose z = %f") %Pose_z # Create an exporter exporter = scan_exporter.ScanExporter() # Create a scanner base base = scanner_base.ScannerBase() #settings for sweep settings = scan_settings.ScanSettings() # Create sweep sensor, and perform scan with Sweep('/dev/ttyUSB0') as sweep: # Create a scanner object time.sleep(1.0) scanner = Scanner( \ device=sweep, base=base, settings=settings, exporter=exporter) # Setup the scanner scanner.setup() # Perform the scan scanner.perform_scan() global epochs epochs -= 1 # Stop the scanner time.sleep(1.0) #scanner.idle() base.turn_off_motors() else: success = False rate = rospy.Rate(0.5) cloud_publisher = rospy.Publisher('/sweep_node/cloudpoint', PointCloud2, queue_size = 1) while not rospy.is_shutdown(): point_cloud2 = pcl2.create_cloud(HEADER, FIELDS, Cloud_Points) #scanner.Points #publish the point cloud message via ros cloud_publisher.publish(point_cloud2) rate.sleep() return Set_Scanning_PoseResponse(success)
def init_scanner(scanner_port): # initialize LiDAR - set speed to 0 Hz try: with Sweep(scanner_port) as sweep: sweep.set_sample_rate(750) sweep.set_motor_speed(0) except RuntimeError as e: print('Scanner error: ' + str(e)) return None
def main(): with Sweep('COM5') as sweep: sweep.start_scanning() # Infinit loop, the loop i executed everytime a new scan is present from the LIDAR for scan in sweep.get_scans(): lidarSamples = scanToNpArray(scan) filterSignalStrength(lidarSamples, 40) filterDistance(lidarSamples, 1, 40000) # histogramSignalStrength(lidarSamples) pass
def run(self): with Sweep(self.dev) as sweep: sweep.start_scanning() for scan in sweep.get_scans(): if self.done.is_set(): self.queue.put_nowait(None) break else: self.queue.put_nowait(scan) sweep.stop_scanning()
def stop_scanner(self): try: with Sweep(self.scanner_port) as sweep: sweep.set_motor_speed(0) print("Scanner stopped") except RuntimeError as e: print('Scanner error: ' + str(e)) self.client_comm.send( bytes( "{\"cmd\": \"direct_print\", \"payload\":{\"text\": \"Scanner error: " + str(e) + "\"} }", 'utf-8'))
def run(self): if __if_sweeppy__: with Sweep(self.dev) as sweep: sweep.start_scanning() for scan in sweep.get_scans(): if self.done.is_set(): #sweep.set_motor_speed(0) self.queue.put_nowait(None) break else: self.queue.put_nowait(scan) sweep.stop_scanning() else: with Sweep() as sweep: for scan in sweep.get_scans(): if self.done.is_set(): self.queue.put_nowait(None) break else: self.queue.put_nowait(scan)
def get_scanner_parameter(): ''' Function to print scanner parameters ''' with Sweep('/dev/ttyUSB0') as sweep: motor_ready = sweep.get_motor_ready() motor_speed = sweep.get_motor_speed() sample_rate = sweep.get_sample_rate() sweep.get_motor_ready() print('Motor ready: ' + str(motor_ready)) print('Motor speed: ' + str(motor_speed)) print('Sample rate: ' + str(sample_rate)) return { 'motor_ready': motor_ready, 'motor_speed': motor_speed, 'sample_rate': sample_rate }
def main(): if len(sys.argv) < 2: sys.exit('python test.py /dev/ttyUSB0') dev = sys.argv[1] with Sweep(dev) as sweep: speed = sweep.get_motor_speed() rate = sweep.get_sample_rate() print('Motor Speed: {} Hz'.format(speed)) print('Sample Rate: {} Hz'.format(rate)) sweep.start_scanning() # get_scans is coroutine-based generator lazily returning scans ad infinitum for n, scan in enumerate(sweep.get_scans()): print('{}\n'.format(scan)) if n == 3: break
def get_profile(nscan=3): ''' Function to record scan profiles :param nscan: number of scan profile to perform and merge :return: a dataframe containing the data in table format ''' lscan = [] with Sweep('/dev/ttyUSB0') as sweep: if sweep.get_motor_ready(): motor_speed = sweep.get_motor_speed() sample_rate = sweep.get_sample_rate() sweep.start_scanning() print('Scan nbr: ') for n, scan in enumerate(sweep.get_scans()): print n, lscan.append(scan) if n == nscan: sweep.stop_scanning() print(' ') print("======================") break if lscan.__len__() > 0: measurements = [] for scan in lscan: for sample in scan.samples: measurements.append(sample) pdm = pd.DataFrame(measurements) pdm.columns = ['angle', 'distance', 'signal_strength'] pdm['timestamp'] = int(time.time()) pdm['sample_rate'] = sample_rate pdm['motor_speed'] = motor_speed return pdm else: return
def start_scanner(self, speed, rate): try: print(type(speed)) with Sweep(self.scanner_port) as sweep: sweep.set_motor_speed(2) sweep.set_sample_rate(750) for i in range( 20): # wait until the scanner is ready or 20 seconds ready = sweep.get_motor_ready() if ready: print('scanner ready to scan') print(sweep.get_sample_rate()) break else: time.sleep(1) self.scanning_done = threading.Event( ) # flag, which will be set when the request to finish scanning comes fifo = queue.Queue() # queue for scans # threads used for getting data from the scanner and getting it from the queue scanner = Scanner(self.scanner_port, fifo, self.scanning_done) getter = ScanGetter(fifo, self.scan_file, self.client_comm, True) # start scanning scanner.start() getter.start() except RuntimeError as e: print('Scanner error: ' + str(e)) self.client_comm.send( bytes( "{\"cmd\": \"direct_print\", \"payload\":{\"text\": \"Scanner error: " + str(e) + "\"} }", 'utf-8'))
import time from sweeppy import Sweep with Sweep('/dev/cu.usbserial-DM00L6FP') as sweep: sweep.start_scanning() angle = 0 angleAtMinDistance = 0 minDistanceQuadrant = 0 minDistance = 50 minDistanceCopy = minDistance distance = 0 rotationalScan = 4 currentRotation = 0 # def resultsOfScan(): # currentRotation = 0 # minDistance = minDistanceCopy # print "Min Angle : {}, Distance : {}, In Quadrant: {}".format(angleAtMinDistance, minDistance, minDistanceQuadrant) # #print("Min Angle: ") for scan in sweep.get_scans(): #print('{}\n'.format(scan)) for s in scan.samples: # print "Angle : {}, Distance : {}".format(s.angle/1000,s.distance) angle = s.angle / 1000 distance = s.distance #check if in quadrant 1
Sweeps.append(d) rs = dict() rs['bSensorIsMobile'] = False rs['bLogTimeStampPerSweep'] = True rs['Sweeps'] = Sweeps with open(filename, 'w') as fp: json.dump(rs, fp) from itertools import islice if __name__ == '__main__': # with Sweep('/dev/ttyUSB0') as sweep: with Sweep('/dev/tty.usbserial-DM00KZW7') as sweep: print('start_scanning') sweep.start_scanning() print('sleep(5)') sleep(2) scan1 = sweep.get_scans() # scan = scan1.__next__() # for scan in islice(sweep.get_scans(), 3): # # write_csv(scan) # scans = list(islice(sweep.get_scans(), 10)) scans = [] for x in range(0, 21): scan = scan1.__next__() scans.append(scan) # write_csv(scans)
def main(): """Tests the sweep's basic functions""" with Sweep('/dev/ttyUSB0') as sweep: output_json_message({ 'type': "update", 'status': "setup", 'msg': "Testing motor ready." }) sweep.get_motor_ready() time.sleep(0.1) output_json_message({ 'type': "update", 'status': "setup", 'msg': "Adjusting sample rate." }) sweep.set_sample_rate(sweep_constants.SAMPLE_RATE_500_HZ) time.sleep(0.1) output_json_message({ 'type': "update", 'status': "setup", 'msg': "Adjusting motor speed." }) sweep.set_motor_speed(sweep_constants.MOTOR_SPEED_5_HZ) time.sleep(0.1) output_json_message({ 'type': "update", 'status': "setup", 'msg': "Testing motor ready." }) sweep.get_motor_ready() time.sleep(0.1) output_json_message({ 'type': "update", 'status': "setup", 'msg': "Starting data acquisition." }) sweep.start_scanning() time.sleep(0.1) desired_num_sweeps = 3 for scan_count, scan in enumerate(sweep.get_scans()): # note that a scan was received (used to avoid the timeout) output_json_message({ 'type': "update", 'status': "setup", 'msg': "Received Scan." }) if scan_count == desired_num_sweeps: break time.sleep(0.1) output_json_message({ 'type': "update", 'status': "setup", 'msg': "Stopping data acquisition." }) sweep.stop_scanning() time.sleep(0.1) output_json_message({ 'type': "update", 'status': "complete", 'msg': "Finished Test!" })
arduino.write("m0\n") print("Throttle off") if event.key == pygame.K_a: print("Turning left") elif event.key == pygame.K_d: arduino.write("Turning right") if event.key == pygame.K_q: arduino.close() pygame.quit() sys.exit() #Initialize the sweep sensor with Sweep('COM4') as sweep: #Set the sweep motor speeds and start scanning sweep.set_motor_speed(5) sweep.set_sample_rate(500) sweep.start_scanning() speed = 0 target = 0 summation = 0 angleAvg = 0 numAngles = 0 position = 0 while True: pygame.font.init()
DEV = os.environ['SCANSE_SWEEP_PORT'] # Create figure: fig1 = plt.figure(1) # Clear figure: plt.clf() # Enable interactive mode: plt.ion() # Create polar subplot: ax = plt.subplot(111, projection='polar') if __name__ == "__main__": with Sweep(DEV) as sweep: speed = sweep.get_motor_speed() rate = sweep.get_sample_rate() print('Motor Speed: {} Hz'.format(speed)) print('Sample Rate: {} Hz'.format(rate)) # Starts scanning as soon as the motor is ready sweep.start_scanning() # get_scans is coroutine-based generator lazily returning scans ad infinitum for scan in sweep.get_scans(): # Create angle and distance lists: angles = [] distances = [] for smp in scan.samples:
## Arm drone ##droneArm(vehicle) ##print("Drone take off...") ## Drone take off - test take of to 10 meters ##droneTakeOff(10, vehicle) ## Wait until start command is send meanAngle = 0 countZ = 100 radiusAnglesEllipse, ellipsePointsPos, bigSmallRadius = getProperRadii( heightMeasured, largeRArray, smallRArray, angleOffsetMeasured) if runProgram is True: with Sweep(portDictionary['4']) as sweep: print('Changing settings to\nMotor Speed\t{} Hz\nSample Rate\{} Hz'. format(motorSpeed, sampleRate)) scanse.changeSettings(sweep, motorSpeed, sampleRate) print('Starting scanning') sweep.start_scanning() try: for scan in sweep.get_scans(): lidarSamples = scanse.scanToNpArray(scan) lidatSamplesFiltered, lidarSamplesBad = scanse.filterSignalStrength( lidarSamples, signalStrengthValue) lidatSamplesFiltered, _ = scanse.filterDistance( lidatSamplesFiltered, distance_min_threshold, distance_max_threshold)
right = True if event.key == pygame.K_q: arduino.close() pygame.quit() sys.exit() if event.key == pygame.K_s: if disarm: disarm = False else: disarm = True #Initialize the sweep sensor with Sweep('COM5') as sweep: #Set the sweep motor speeds and start scanning sweep.set_motor_speed(10) sweep.set_sample_rate(500) sweep.start_scanning() position = 0 for scan in sweep.get_scans(): screen.fill(WHITE) #print(right); summation = 0 num_distances = 0
def scan(): global stop point_list = [] slope_list = [] while True: print('Recording measurements... Press Crl+C to stop.') lasttime = time.time() with Sweep(dev) as sweep: speed = sweep.get_motor_speed() rate = sweep.get_sample_rate() print('Motor Speed: {} Hz'.format(speed)) print('Sample Rate: {} Hz'.format(rate)) # Starts scanning as soon as the motor is read sweep.start_scanning() # get_scans is coroutine-based generator lazily returning scans ad infinitum for scan in itertools.islice(sweep.get_scans(), 3): print(scan) if time.time() < (lasttime + 0.1): # [0] -> New # [1] -> Quality # [2] -> Angle (0 - 360) # [3] -> Distance in mm print("time: ", time.time()) #print("lasttime: ", lasttime) # if((measurment[3] > 100) and (measurment[3]<2500) and (measurment[1]>2) and ((measurment[2] < 135) or (225 < measurment[2]))): # x_pos = math.cos(math.radians(measurment[2])) * measurment[3] # in mm # y_pos = math.sin(math.radians(measurment[2])) * measurment[3] # in mm # point_list.append((x_pos, y_pos)) else: if len(point_list) > 2: for i in range(len(point_list)): this_point = point_list[i] for j in range(len(point_list) - 1 - i): other_point = point_list[1 + i] mx = this_point[0] - other_point[0] my = this_point[1] - other_point[1] temp = math.atan2(my,mx) if not math.isnan(temp) and not math.isinf(temp): slope_list.append(temp) slope_list = sorted(slope_list) median = math.degrees(slope_list[len(slope_list)/2]) median = median % 180 # forcing output to be between 0 and 179 if median < 0: median += 180 slope_sum = 0 for j in range(len(slope_list)): temp2 = slope_list[j] % 180 if temp2 < 0: temp2 += 180 temp2 = temp2 - median temp2 = temp2*temp2 slope_sum = slope_sum + temp2 slope_sum = slope_sum / len(slope_list) slope_sum = math.sqrt(slope_sum) # print("Slope sum: ",slope_sum) median = 10 * (1/math.tan(math.radians(median))) # Teensy_Port.write("%f\n" % median) print (median) point_list = [] slope_list = [] lasttime = time.time()
def main(): """Sets the sweep to idle""" with Sweep('/dev/ttyUSB0') as sweep: sweep.set_motor_speed(sweep_constants.MOTOR_SPEED_0_HZ)
class LidarTankPlatform: DEV = os.environ['SCANSE_SWEEP_PORT'] WIDTH = 30. FORWARD = 100. # Create lists: analysis_data = dataserver.analysis_data def __init__(self, *args, **kwargs): self.sweep = None self.motors = DaguRS039(*args, **kwargs) self._running = None self.st = threading.Thread(target=self._sweep_data_helper) self.ds = None # Data socket server self.dst = None # Data socket server thread def __enter__(self): self._running = True # Start sweep thread self.sweep = Sweep(self.DEV).__enter__() self.sweep.start_scanning() self.st.start() # Start Sweep thread # Data server: self.ds, self.dst = dataserver.start( ) # Start data server, and store socket server and thread objects return self def __exit__(self, *args, **kwargs): self._running = False # Sweep close self.st.join() # Wait Sweep thread to end self.sweep.__exit__(*args, **kwargs) # Data server: self.ds.close() # Close socket server self.dst.join() # Wait for loop thread to close # Gets data from sweep: def _sweep_data_helper(self): for scan in self.sweep.get_scans(): if not self._running: return tmp_sweep_wnt = [] tmp_sweep_otr = [] for smp in scan.samples: a = (((smp.angle / 1000) + 135) % 360) * (math.pi / 180) d = smp.distance # cm cart_p = [math.cos(a) * d, math.sin(a) * d] if -self.WIDTH / 2. < cart_p[ 0] < self.WIDTH / 2. and 0. < cart_p[1] < self.FORWARD: tmp_sweep_wnt.append([cart_p[0], cart_p[1]]) # print("conn: {}, d: {}, x: {}, y: {}".format(conn, d, cart_p[0], cart_p[1])) else: tmp_sweep_otr.append([cart_p[0], cart_p[1]]) self.analysis_data['sweep_wnt'] = tmp_sweep_wnt self.analysis_data['sweep_otr'] = tmp_sweep_otr
endAngle = TARGET + TARGETBUFFER if (startAngle < 0): startAngle += 360 if (endAngle > 360): endAngle -= 360 adjust = 315 - TARGET def within(a): if (startAngle < endAngle): return a > startAngle and a < endAngle if (startAngle > endAngle): return a > startAngle or a < endAngle if (MODE): print("Using LiDAR") with Sweep('/dev/ttyUSB0') as sweep: sweep.set_motor_speed(2) sweep.set_sample_rate(1000) sweep.start_scanning() first = True for scan in itertools.islice(sweep.get_scans(), 3): if (not first): s = scan[0] for dataSample in s: ang = dataSample[0] / 1000.0 if (within(angle)): angle.append(ang) distance.append(dataSample[1]) break
def sigint_handler(sweep, sig, frame): print("Lidar shut down sequence initialized") sweep.stop_scanning() sweep.set_motor_speed(0) print("Lidar shut down sequence finished") sys.exit(0) pub = PubSocket(cfg) initial_motor_speed = cfg.get('motor_speed', 1) initial_sample_rate = cfg.get('sample_rate', 500) print("Initial speed: {}, sampling rate: {}".format(initial_motor_speed, initial_sample_rate)) with Sweep(cfg['tty']) as sweep: signal.signal(signal.SIGINT, partial(sigint_handler, sweep)) sweep.set_motor_speed(initial_motor_speed) sweep.set_sample_rate(initial_sample_rate) while not sweep.get_motor_ready(): print("Waiting for a lidar motor to become ready") time.sleep(1) print("Actual motor speed: {}, sampling rate: {}".format(sweep.get_motor_speed(), sweep.get_sample_rate())) sweep.start_scanning() print("All set, liftoff") pub.broadcast(TranslateReadings(sweep))