VELODYNE_PORT = 2368
lidar_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#lidar_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
lidar_sock.bind(("0.0.0.0", VELODYNE_PORT))
print('Attempting to capture data from a Velodyne VLP-16 Lidar...')
tLast = time.time()
lastSize = 0
f = open(args.filename, 'wb')
while True:
inputs, outputs, errors = select.select([lidar_sock], [], [], 1.0)
for oneInput in inputs:
if oneInput == lidar_sock:
pkt, addr = get_last_packet(lidar_sock, 2048, verbose=False)
if len(pkt) == 1206:
pickle.dump(('vlp16', time.time(), addr, pkt), f, -1)
else:
print('Wrong packet size: %d bytes' % (len(pkt)))
# Send status updates to the screen:
ts = time.time()
tDelta = ts - tLast
if tDelta > 1.0:
tLast = ts
speed = (f.tell() - lastSize) / tDelta
lastSize = f.tell()
print(sizeof_fmt(f.tell()), '(%s)' % (sizeof_fmt(speed, suffix='B/s')))
est_lon = nmea.lon
if nmea.true_course is not None:
# only update if we are driving straight forward:
if np.abs(imu.sbus_a -
1024) < 20 and imu.sbus_b > 1030:
calcHeading.calibrate_from_gps(nmea.true_course)
mav_gps_fix_type = nmea.get_mavlink_fix()
mavlink.send_raw_gps(nmea.ts_us, mav_gps_fix_type,
nmea.lat, nmea.lon, nmea.alt,
nmea.speed, nmea.GGA_numsats)
mavlink.send_gps(est_lat, est_lon, nmea.alt)
elif oneInput == imu_sock:
pkt, addr = get_last_packet(imu_sock, 128)
try:
imu.parse(pkt)
except Exception as ee:
print('failed to parse imu packet:', ee)
else:
rot = transforms3d.quaternions.quat2mat(
[imu.qw, imu.qx, imu.qy, imu.qz])
if IMU_XFRM is not None:
rot = np.dot(rot, IMU_XFRM)
pitch, roll, _yaw = transforms3d.euler.mat2euler(rot)
calcHeading.update_gyro_yaw(-_yaw)
#calcHeading.update_magnet_yaw(imu.magX, imu.magY, imu.magZ)
cone1.Direction = np.dot(R, np.array([1,0,0]))
cone2.Direction = np.dot(R, np.array([0,1,0]))
cone3.Direction = np.dot(R, np.array([0,0,1]))
renderView1.Update()
RenderAllViews()
return
x1, y1, z1, x2, y2, z2, x3, y3, z3 = struct.unpack('!fffffffff', data)
cone1.Direction = [x1, y1, z1]
cone2.Direction = [x2, y2, z2]
cone3.Direction = [x3, y3, z3]
renderView1.Update()
RenderAllViews()
while True:
inputs, outputs, errors = select.select([data_sock], [], [])
for oneInput in inputs:
if oneInput == data_sock:
pkt, addr = get_last_packet(data_sock)
if pkt is not None:
if len(pkt) == 8*9:
R = np.frombuffer(pkt)
R = R.reshape((3,3))
updateRotation(R)
else:
print('failed to parse UDP packet')
print(len(pkt))