Nov-17-2023, 06:22 PM
Hello all: Jayson Here; and I have struggled with getting lidar implemented in python, now i have struck gold with my new UniTree lidar. It reads me data to python and puts it in lists....I just do not know how to make the list of quaternions an xyz list yet for graphing....it looks intimidating so i began this topic.
I i have code which i will share here saved to github which retrieves and graphs the lidar data, however....i get two scan messages which must be combined mathematically...one of distance points and one of imu which has the quaternions...with theta roe and phi or some odd bit.....any how as i approach the main question it is how to make an xyz point list in python from quaternions,angular velocity, linear accelleration.......
[b]would the only way to know theta (in the making of the list) from a lidar machine readout or is it compute-able someway.....[/b].
Ie.....(i do not have auxillaryData or , just rangeData.... and a list of quaternions, the imu message)..
https://github.com/Jayson-Tolleson/UniTr...on-Example
I i have code which i will share here saved to github which retrieves and graphs the lidar data, however....i get two scan messages which must be combined mathematically...one of distance points and one of imu which has the quaternions...with theta roe and phi or some odd bit.....any how as i approach the main question it is how to make an xyz point list in python from quaternions,angular velocity, linear accelleration.......
[b]would the only way to know theta (in the making of the list) from a lidar machine readout or is it compute-able someway.....[/b].
Ie.....(i do not have auxillaryData or , just rangeData.... and a list of quaternions, the imu message)..
import math def parseRangeAuxiliaryDataToCloud(auxiliaryData, rangeData, scanCloud): if auxiliaryData.packet_id != rangeData.packet_id: return False rotate_yaw_bias = 0 range_scale = 0.001 z_bias = 0.0445 points_num_of_scan = 120 bias_laser_beam_ = auxiliaryData.b_axis_dist / 1000 sin_theta = math.sin(auxiliaryData.theta_angle) cos_theta = math.cos(auxiliaryData.theta_angle) sin_ksi = math.sin(auxiliaryData.ksi_angle) cos_ksi = math.cos(auxiliaryData.ksi_angle) pitch_cur = auxiliaryData.sys_vertical_angle_start * math.pi / 180.0 pitch_step = auxiliaryData.sys_vertical_angle_span * math.pi / 180.0 yaw_cur = (auxiliaryData.com_horizontal_angle_start + rotate_yaw_bias) * math.pi / 180.0 yaw_step = auxiliaryData.com_horizontal_angle_step / points_num_of_scan * math.pi / 180.0 range_float = 0 point = [0, 0, 0, 0] for i in range(0, points_num_of_scan * 2, 2): j = i // 2 pitch_cur += pitch_step yaw_cur += yaw_step range = (rangeData.point_data[i + 1] << 8) | rangeData.point_data[i] if range == 0: continue range_float = range_scale * range sin_alpha = math.sin(pitch_cur) cos_alpha = math.cos(pitch_cur) sin_beta = math.sin(yaw_cur) cos_beta = math.cos(yaw_cur) A = (-cos_theta * sin_ksi + sin_theta * sin_alpha * cos_ksi) * range_float + bias_laser_beam_ B = cos_alpha * cos_ksi * range_float point[0] = cos_beta * A - sin_beta * B point[1] = sin_beta * A + cos_beta * B point[2] = (sin_theta * sin_ksi + cos_theta * sin_alpha * cos_ksi) * range_float + z_bias point[3] = auxiliaryData.reflect_data[j] scanCloud.append(point) return Truethis is my(working) code from unitree (mostly) that gets data via udp
import socket import struct import time import sys import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import axes3d import pydeck import pandas as pd import numpy as np import subprocess from subprocess import Popen, PIPE, STDOUT, run # IP and Port UDP_IP = "0.0.0.0" UDP_PORT = 80 #cloud_scan_num = str(np.clip(int(sys.argv[1]), 0, 20000)) #subprocess.Popen('./unilidar_publisher_udp '+str(cloud_scan_num), shell=True, stdout=subprocess.PIPE, stderr=STDOUT) ################################################################################## def plot(x,y,z,intensity): fig = plt.figure(figsize = (12, 10)) ax = plt.axes(projection ="3d") ax.grid(visable = True, color ='grey',linestyle ='-.', linewidth = 0.3, alpha = 0.2) my_cmap = plt.get_cmap('tab20b') sctt = ax.scatter3D(x, y, z, alpha = 1, c = z, cmap = my_cmap, marker ='.') plt.title("Lidar Scan 3D scatter plot") fig.colorbar(sctt, ax = ax, shrink = 0.5, aspect = 5) plt.savefig('/home/jay/Documents/unitree-lidar/WorkingUnilidar/static/xyz.jpg') ############################################################################# def plot2(pointlist): pointlist2 = pd.DataFrame(pointlist, columns=['x', 'y', 'z','r','g','b','i']) target = [pointlist2.x.mean(), pointlist2.y.mean(), pointlist2.z.mean()] point_cloud_layer = pydeck.Layer( "PointCloudLayer", data=pointlist2, get_position=["x", "y", "z"], get_color=["r", "g", "b"], get_normal=[0, 0, 15], auto_highlight=True, pickable=True, point_size=3,) view_state = pydeck.ViewState(target=target, controller=True, rotation_x=15, rotation_orbit=30, zoom=10) view = pydeck.View(type="OrbitView", controller=True) r = pydeck.Deck(point_cloud_layer, initial_view_state=view_state, views=[view]) r.to_html("/home/jay/Documents/unitree-lidar/WorkingUnilidar/static/point_cloud_layer.html", css_background_color="#add8e6") ############################################################################# # Point Type class PointUnitree: def __init__(self, x, y, z, intensity, time, ring): self.x = x self.y = y self.z = z self.intensity = intensity self.time = time self.ring = ring # Scan Type class ScanUnitree: def __init__(self, stamp, id, validPointsNum, points): self.stamp = stamp self.id = id self.validPointsNum = validPointsNum self.points = points # IMU Type class IMUUnitree: def __init__(self, stamp, id, quaternion, angular_velocity, linear_acceleration): self.stamp = stamp self.id = id self.quaternion = quaternion self.angular_velocity = angular_velocity self.linear_acceleration = linear_acceleration # Create UDP socket sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((UDP_IP, UDP_PORT)) # Calculate Struct Sizes imuDataStr = "=dI4f3f3f" imuDataSize = struct.calcsize(imuDataStr) pointDataStr = "=fffffI" pointSize = struct.calcsize(pointDataStr) scanDataStr = "=dII" + 120 * "fffffI" scanDataSize = struct.calcsize(scanDataStr) print("pointSize = " +str(pointSize) + ", scanDataSize = " + str(scanDataSize) + ", imuDataSize = " + str(imuDataSize)) while True: # Recv data data, addr = sock.recvfrom(10000) print(f"Received data from {addr[0]}:{addr[1]}") msgType = struct.unpack("=I", data[:4])[0] print("msgType =", msgType) if msgType == 101: # IMU Message length = struct.unpack("=I", data[4:8])[0] imuData = struct.unpack(imuDataStr, data[8:8+imuDataSize]) imuMsg = IMUUnitree(imuData[0], imuData[1], imuData[2:6], imuData[6:9], imuData[9:12]) quaternion = imuMsg.quaternion ang_vel = imuMsg.angular_velocity lin_acc = imuMsg.linear_acceleration print ('quaternion: ', quaternion,'angularVelocity: ',ang_vel,'linearAcceleration: ',lin_acc) time.sleep(14) elif msgType == 102: # Scan Message length = struct.unpack("=I", data[4:8])[0] stamp = struct.unpack("=d", data[8:16])[0] id = struct.unpack("=I", data[16:20])[0] validPointsNum = struct.unpack("=I", data[20:24])[0] scanPoints = [] pointStartAddr = 24 for i in range(validPointsNum): pointData = struct.unpack(pointDataStr, data[pointStartAddr: pointStartAddr+pointSize]) pointStartAddr = pointStartAddr + pointSize point = PointUnitree(*pointData) scanPoints.append(point) scanMsg = ScanUnitree(stamp, id, validPointsNum, scanPoints) print("A Scan msg is parsed!") print("\tstamp =", scanMsg.stamp, "id =", scanMsg.id) print("\tScan size =", scanMsg.validPointsNum) time.sleep(8) print("\tfirst 10 points (x, y, z, intensity, time, ring) =") x = [] y = [] z = [] r = [] g = [] b = [] intensity = [] for i in range(1,len(scanMsg.points)): point = scanMsg.points[i] print("\t", point.x, point.y, point.z, point.intensity, point.time, point.ring) x.append(point.x) y.append(point.y) z.append(point.z) intensity.append(point.intensity) r.append(64) g.append(130) b.append(109) print("\n") pointlist = list(zip(x,y,z,r,g,b,intensity)) print (pointlist[:10]) plot(x,y,z,intensity) plot2(pointlist) ############################################################### sock.close()the whole code is available @
https://github.com/Jayson-Tolleson/UniTr...on-Example