Commit 88903ed1 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

stable point

parent 378496fe
......@@ -55,18 +55,23 @@ class LidarSensor(Getter):
j=0.0
dth = 270./len(dat)
elem_arr = []
for i in range(0,len(dat),1):
for i in range(2,len(dat),4):
if(len(dat[i:]) < 4):
break
#th = np.radians(float(dat[i+1]))
th = j
r = float(dat[i])
#th = j
#r = float(dat[i])
#x = float(dat[i])
x = r*math.cos(th)
#x = r*math.cos(th)
#x = i
#y = float(dat[i])
y = r*math.sin(th)
#y = r*math.sin(th)
x = dat[i]/10.+dat[i+1]/100.+dat[i+2]/1000.+dat[i+3]/10000.
y = j
z = 0.05
elem_arr.append([x,y,z])
j = j + np.radians(dth)
j = j + 0.01
#j = j + np.radians(dth)
header = Header()
header.stamp = current_time
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment