stable point

hv_bridge/includes/Sensors/lidar.py

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