ATV Capstone LIDAR files Su2025

ATV Lidar Setup and Documentation.md

@@ -0,0 +1,50 @@
+1.Henry Haight
+
+_University of Washington Bothell_
+
+<henryh73@uw.edu>
+
+**_Abstract—The purpose of this document is to describe how to operate the YDLidar for the Autonomous ATV. This includes setup instructions and written explanations for the functions used by the lidar to detect objects._**
+
+Keywords—template, UWB Electronics Club, format
+
+# Features
+
+- Setup
+- Functions
+- Quirks of the YDLidar
+## Setup
+
+Download the SDK driver for \[YDLIDAR\](<https://github.com/YDLIDAR/YDLidar-SDK/tree/master>)
+
+Check the Linux machine has:
+
+- swig
+- python
+- openpyxl
+- pandas
+- cmake
+- matplotlib
+- Follow the \[instructions for installation on the YDLIDAR Github page\](<https://github.com/YDLIDAR/YDLidar-SDK/blob/master/doc/howto/how_to_build_and_install.md>)
+- Open 'YDLidar-SDK/python/examples/plot\\\_tof\\\_test.py' and change line 26 to this: 'laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000\\)'
+- Run Test '$ cd YDLidar-SDK/python/examples/ $ python plot\\\_tof\\\_test.py'
+
+## Functions
+
+animate(num)
+
+This function is the main function of the LIDAR code. This creates 3 arrays and uses them to append the 3 major components of the LIDAR signal: angle, range, and intensity. These arrays are added to using the YDLidar scan function, which then adds them to each array every pass of the LIDAR. This is then graphed at the end of the function after passing through the other 2 functions, which determine which signals are objects.
+
+The intensity is always set at 10, so it is useless for our application. This seems to be a quirk of the YDLidar API. The angle is measured in radians. Range seems to be measured in feet.
+
+find_close_sequences(angle, threshold=, ran)
+
+This finds where the sequences are close and returns arrays where there are points that are close, so we can fit a line between 3 points of those arrays in graph_points
+
+graph_points(sequences)
+
+Finds the midpoints, start, and end of the 2D array, which is then returned as one array so it can be graphed
+
+## Quirks of the Lidar
+
+To use your own code place the file you want to modify or make in YDLidar-SDK/python/examples/. This allows the lidar to find the library it needs to start and call the lidar.

YDLIDAR_ATV.py

@@ -0,0 +1,81 @@
+import os
+import ydlidar
+import time
+import sys
+from matplotlib.patches import Arc
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import numpy as np
+import pandas as pd
+
+RMAX = 32.0
+
+
+fig = plt.figure()
+fig.canvas.set_window_title('YDLidar LIDAR Monitor')
+lidar_polar = plt.subplot(polar=True)
+lidar_polar.autoscale_view(True,True,True)
+lidar_polar.set_rmax(RMAX)
+lidar_polar.grid(True)
+ports = ydlidar.lidarPortList();
+port = "/dev/ydlidar";
+for key, value in ports.items():
+    port = value;
+
+laser = ydlidar.CYdLidar();
+laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
+laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000)
+laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF);
+laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
+laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
+laser.setlidaropt(ydlidar.LidarPropSampleRate, 20);
+laser.setlidaropt(ydlidar.LidarPropSingleChannel, False);
+laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
+laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
+laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0);
+laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01);
+scan = ydlidar.LaserScan()
+
+dfa = pd.DataFrame()
+dfr = pd.DataFrame()
+dfi = pd.DataFrame()
+
+
+def animate(num):
+
+    r = laser.doProcessSimple(scan);
+    if r:
+        angle = []
+        ran = []
+        intensity = []
+
+        for point in scan.points:
+            angle.append(point.angle);
+            ran.append(point.range);
+            intensity.append(point.intensity);
+
+
+        lidar_polar.clear()
+        lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', alpha=0.95)
+
+
+        dfa = pd.DataFrame({'Angle': angle, 'Ran': ran, 'Intensity': intensity})
+	dfa.to_excel('angle.xlsx', index=False)
+	#dfr = pd.DataFrame(ran)
+	#dfr.to_excel('ran.xlsx', index=False)
+	#dfi = pd.DataFrame(angle)
+	#dfi.to_excel('intensity.xlsx', index=False)
+
+ret = laser.initialize();
+if ret:
+    ret = laser.turnOn();
+    if ret:
+        ani = animation.FuncAnimation(fig, animate, interval=50)
+        plt.show()
+    laser.turnOff();
+
+
+
+laser.disconnecting();
+plt.close();
+

YDLIDAR_ATV_obstacle.py

@@ -0,0 +1,108 @@
+import os
+import ydlidar
+import time
+import sys
+from matplotlib.patches import Arc
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import numpy as np
+import pandas as pd
+
+
+RMAX = 32.0
+
+
+fig = plt.figure()
+fig.canvas.set_window_title('YDLidar LIDAR Monitor')
+lidar_polar = plt.subplot(polar=True)
+lidar_polar.autoscale_view(True,True,True)
+lidar_polar.set_rmax(RMAX)
+lidar_polar.grid(True)
+ports = ydlidar.lidarPortList();
+port = "/dev/ydlidar";
+for key, value in ports.items():
+    port = value;
+
+laser = ydlidar.CYdLidar();
+laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
+laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000)
+laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF);
+laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
+laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
+laser.setlidaropt(ydlidar.LidarPropSampleRate, 20);
+laser.setlidaropt(ydlidar.LidarPropSingleChannel, False);
+laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
+laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
+laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0);
+laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01);
+scan = ydlidar.LaserScan()
+
+def find_close_sequences(angle, ran, threshold=0.015):
+    i = 0
+    angle_sequences = []
+    ran_sequences = []
+
+    while i < len(angle) - 1:
+        start = i
+        while i < len(angle) - 1 and abs(angle[i] - angle[i + 1]) <= threshold:
+            i += 1
+        if i > start:
+            angle_sequences.append(angle[start:i + 1])
+            ran_sequences.append(ran[start:i + 1])
+        i += 1
+
+    return angle_sequences, ran_sequences
+
+def graph_points(sequences):
+    points = []
+
+    for i, seq in enumerate(sequences):
+        start = sequences[i][0]
+        end = start + len(seq) - 1
+        mid = (start + end) // 2.0
+	points.append([start, mid, end])
+
+    return points
+
+
+
+def animate(num):
+
+    r = laser.doProcessSimple(scan);
+    if r:
+        angle = []
+        ran = []
+        intensity = []
+
+        for point in scan.points:
+		if  point.range < 2.0:
+           	    angle.append(point.angle);
+          	    ran.append(point.range);
+         	    intensity.append(point.intensity);
+        angle_objects, ran_objects = find_close_sequences(angle, ran, threshold=0.015)
+
+        angle_gp = graph_points(angle_objects)
+        ran_gp = graph_points(ran_objects)
+
+
+        lidar_polar.clear()
+        #lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', alpha=0.95)
+	plt.plot(angle, ran)
+
+	dfa = pd.DataFrame({'Angle': angle_gp, 'Range': ran_gp})
+	dfa.to_excel('data.xlsx', index=False)
+
+
+
+ret = laser.initialize();
+if ret:
+    ret = laser.turnOn();
+    if ret:
+        ani = animation.FuncAnimation(fig, animate, interval=50)
+        plt.show()
+    laser.turnOff();
+
+laser.disconnecting();
+plt.close();
+
+

YDLIDAR_ATV_obstacle.py-LK_Version.py

@@ -0,0 +1,121 @@
+import os
+import ydlidar
+import time
+import sys
+from matplotlib.patches import Arc
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import numpy as np
+import pandas as pd
+
+
+RMAX = 32.0
+
+
+fig = plt.figure()
+fig.canvas.set_window_title('YDLidar LIDAR Monitor')
+lidar_polar = plt.subplot(polar=True)
+lidar_polar.autoscale_view(True,True,True)
+lidar_polar.set_rmax(RMAX)
+lidar_polar.grid(True)
+ports = ydlidar.lidarPortList();
+port = "/dev/ydlidar";
+for key, value in ports.items():
+    port = value;
+
+laser = ydlidar.CYdLidar();
+laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
+laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000)
+laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF);
+laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
+laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
+laser.setlidaropt(ydlidar.LidarPropSampleRate, 20);
+laser.setlidaropt(ydlidar.LidarPropSingleChannel, False);
+laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
+laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
+laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0);
+laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01);
+scan = ydlidar.LaserScan()
+
+def find_close_sequences(angle, ran, threshold=0.015):
+    i = 0
+    angle_sequences = []
+    ran_sequences = []
+
+    while i < len(angle) - 1:
+        start = i
+        while i < len(angle) - 1 and abs(angle[i] - angle[i + 1]) <= threshold:
+            i += 1
+        if i > start:
+            angle_sequences.append(angle[start:i + 1])
+            ran_sequences.append(ran[start:i + 1])
+        i += 1
+
+    return angle_sequences, ran_sequences
+
+def graph_points(sequences):
+    points = []
+
+    for i, seq in enumerate(sequences):
+        start = 0
+        end = start + len(seq) - 1
+        mid = (start + end) // 2
+        points.append([start, mid, end])
+
+    return points
+
+
+
+def animate(num):
+
+    r = laser.doProcessSimple(scan);
+    if r:
+        angle = []
+        ran = []
+        intensity = []
+        obs = []
+        obs2 = []
+
+        for point in scan.points:
+            if point.range < 5:
+                angle.append(point.angle)
+                ran.append(point.range)
+                intensity.append(point.intensity)
+
+
+        angle_objects, ran_objects = find_close_sequences(angle, ran, threshold=0.015)
+
+        angle_gp = graph_points(angle_objects)
+        ran_gp = graph_points(ran_objects)
+
+
+        lidar_polar.clear()
+        lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', alpha=0.95)
+
+	if point.range < .01:
+		print("Number of objects detected: ", len(angle_objects));
+        	print("Object detected, shutting down the program...");
+
+        	for i in range(len(angle_objects)):
+         	   lidar_polar.plot(angle_objects[i], ran_objects[i], marker='o', markersize=3, 	linestyle='-', linewidth=1, alpha=0.5)
+        	   lidar_polar.plot([angle_gp[i][0], angle_gp[i][1], angle_gp[i][2]], [ran_gp[i][0], ran_gp[i][1], ran_gp[i][2]], marker='o', markersize=3, linestyle='-', linewidth=1, alpha=0.5)
+   	     	sys.exit();
+
+
+    dfa = pd.DataFrame({'Angle': angle_objects, 'Range': ran_objects})
+    dfa.to_excel('data.xlsx', index=False)
+
+
+
+ret = laser.initialize();
+if ret:
+    ret = laser.turnOn();
+    if ret:
+        ani = animation.FuncAnimation(fig, animate, interval=50)
+        plt.show()
+    laser.turnOff();
+
+laser.disconnecting();
+plt.close();
+
+