def polarScan(num_points=54): # You may request up to 811 points, max.
lidar.scan() #take reading
# LIDAR data properties
dist_amnt = lidar.scan.dist_data_amnt # Number of distance data points reported from the lidar
angle_res = lidar.scan.dist_angle_res # Angular resolution reported from lidar
# create the column of distances
scan_points = np.asarray(lidar.scan.distances) #store the reported readings and cast as numpy.array
inc_ang = (dist_amnt/(num_points+1))*angle_res # Calculate angle increment for scan_points resized to num_points
scan_points = np.asarray(np.array_split(scan_points,num_points)) # Split array into sections
scan_points = [item[0] for item in scan_points] # output first element in each section into a list
scan_points = np.asarray(scan_points) # cast the list into an array
scan_points = np.reshape(scan_points,(scan_points.shape[0],1)) # Turn scan_points row into column
#create the column of angles
angles = np.zeros(num_points)
x = len(angles)
for i in range(x): #run this loop
angles[i] = (i*lidar.scan.dist_angle_res*lidar.scan.dist_data_amnt/num_points)+(start_angle)
angles = np.reshape(angles,(angles.shape[0],1)) # Turn angles row into column
#create the polar coordinates of scan
scan_points = np.hstack((scan_points,angles)) # Turn two (54,) arrays into a single (54,2) matrix
scan_points = np.round(scan_points,3) # Round each element in array to 3 decimal places
return(scan_points)
def nearest_point():
lidar.scan() #take reading
# LIDAR data properties
dist_amnt = lidar.scan.dist_data_amnt # Number of distance data points reported from the lidar
angle_res = lidar.scan.dist_angle_res # Angular resolution reported from lidar
# create the column of distances
scan_points = np.asarray(lidar.scan.distances) #store the reported readings and cast as numpy.array
inc_ang = (dist_amnt/(num_points+1))*angle_res # Calculate angle increment for scan_points resized to num_points
scan_points = np.asarray(np.array_split(scan_points,num_points)) # Split array into sections
scan_points = [item[0] for item in scan_points] # output first element in each section into a list
scan_points = np.asarray(scan_points) # cast the list into an array
scan_points = np.reshape(scan_points,(scan_points.shape[0],1)) # Turn scan_points row into column
#create the column of angles
angles = np.zeros(54)
x = len(angles)
for i in range(x): #run this loop
angles[i] = (i*lidar.scan.dist_angle_res*lidar.scan.dist_data_amnt/num_points)+(start_angle)
angles = np.reshape(angles,(angles.shape[0],1)) # Turn angles row into column
#create the polar coordinates of scan
scan_points = np.hstack((scan_points,angles)) # Turn two (54,) arrays into a single (54,2) matrix
scan_points = np.round(scan_points,3) # Round each element in array to 3 decimal places
# convert the polar coordinates into cartesian
scan_cart = np.zeros((num_points,2))
d = len(scan_cart) # return the number of elements in scan
for d in range(d):
scan_cart[d,0]=scan_points[d,0]*np.cos(np.radians(scan_points[d,1]))
scan_cart[d,1]=scan_points[d,0]*np.sin(np.radians(scan_points[d,1]))
# calculate the distances between p0 and scan x_points
vector_lengths = np.zeros(num_points)
k = 0.004 # minimum distance for a reading considered to be valid
d = len(scan_cart) #return the number of elements in scan
for d in range(d):
a = scan_cart[d,0]-p1[0] # difference in x values
b = scan_cart[d,1]-p1[1] # difference in y values
c = np.sqrt(a**2 + b**2) # take the hypotenuse
if c < k:
c = 5.0 # for nonvalid measurements, raise to 5 meters
vector_lengths[d]= c # assign hypotenuse to vector length
#"filter" returns array of all values greater than k, and min finds the minimum
d_min = min(filter(lambda x: x > k, vector_lengths))
myIndex = np.argmin(vector_lengths)
myYValue = scan_cart[myIndex,1] #column 1 is y values in scan_cart
myPoint = np.array([ d_min, myYValue ])
myPoint = np.round(myPoint, decimals = 3)
#print(d_min) #print out the closest detected obstacle
return(myPoint)
import pysicktim as lidar # data = lidar.demo_data(file="demo_data_test.dat") # data = lidar.demo_data(file="demo_data_test_empty.dat") lidar.scan(demo=True, file="demo_data_test.dat") print(data)
# Import pysicktim library as "lidar"
import pysicktim as lidar
import numpy as np
import L2_log as log
# Repeat code nested in for loop 10 times
for x in range(1):
# while True:
lidar.scan() # Requests and returns list of LiDAR
# distance data in meters
scan_points = np.asarray(lidar.scan.distances)
a = scan_points[338:474:1]
log.csv_write(a)
print("sum is", a.sum())
print("std is", np.std(a))
# print distance list
def get_distance(): #returns array
lidar.scan() # Requests and returns list of LiDAR
# distance data in meters
scan_points = np.asarray(lidar.scan.distances) # assign all points to scan_points
sweep_dis = scan_points[300:500:1] # a = points on the array from 338 - 474
return sweep_dis #sweep_dis = 0.4 m sweep (distances)
import socket
import pysicktim as lidar
port = 9999
socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
socket.bind(("", port))
while 1:
try:
request, ip = socket.recvfrom(1024)
lidar.scan()
packet = str(int(lidar.scan.dist_start_ang)) , str(int(lidar.scan.dist_angle_res*10000)) , str(lidar.scan.dist_data_amnt), lidar.scan.raw_distances
packet = " ".join(packet)
socket.sendto(packet.encode(), ip)
except KeyboardInterrupt:
exit()
except:
socket.sendto(packet.encode(), ip)
pass
# -*- coding: utf-8 -*-
import os
import pysicktim as lidar # Import TIM5xx Library as "lidar"
os.system('clear')
data = lidar.scan()
print("Telegram Length: ", lidar.scan.telegram_len, "\n")
print("Command Type:\t", lidar.scan.cmd_type)
print("Command :\t", lidar.scan.cmd)
print("Version Num :\t", lidar.scan.version)
print("Device Num :\t", lidar.scan.device_num)
print("Serial Num :\t", lidar.scan.serial_num)
print("Device Stat :\t", lidar.scan.device_stat)
print("Telegrm Cnt :\t", lidar.scan.telegram_cnt)
print("Scan Count :\t", lidar.scan.scan_cnt)
print("\nTime since start (\u03BCs):\t\t", lidar.scan.uptime)
print("Time of transmission (\u03BCs):\t", lidar.scan.trans_time)
print("Digital Inputs Status:\t\t", lidar.scan.input_stat)
print("Digital Outputs Status:\t\t", lidar.scan.output_stat)
print("Layer Angle :\t\t\t", lidar.scan.layer_ang)
print("Scan Frequency (Hz):\t\t", lidar.scan.scan_freq)
print("Measurement Frequency (Hz):\t", lidar.scan.meas_freq)
print("Amount of Encoder Data :\t", lidar.scan.enc_amount)
print("# of valid 16 bit channels :\t", lidar.scan.num_16bit_chan)
print("\n\tDistance\n")