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