From 3aac12f7cb78398562c9d8cc75b7b89092580612 Mon Sep 17 00:00:00 2001 From: DanielRhee Date: Thu, 30 Oct 2025 08:39:45 -0700 Subject: [PATCH 01/18] Path planning --- PathPlanning/README.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 PathPlanning/README.md diff --git a/PathPlanning/README.md b/PathPlanning/README.md new file mode 100644 index 0000000..e69de29 From ed9bd66e6eca9559be10afb2bc9098a6d77ef549 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Sun, 9 Nov 2025 22:42:44 -0800 Subject: [PATCH 02/18] delaunay.py --- PathPlanning/delaunay.py | 74 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 PathPlanning/delaunay.py diff --git a/PathPlanning/delaunay.py b/PathPlanning/delaunay.py new file mode 100644 index 0000000..813d330 --- /dev/null +++ b/PathPlanning/delaunay.py @@ -0,0 +1,74 @@ +from scipy.spatial import Delaunay +import numpy as np +import matplotlib.pyplot as plt + +def parse_cone_data(cones): + # Parses cone data from mapping team to create points to use for triangulation + + return np.array(cones) + + +def create_delaunay_triangulation(points): + # Create a Delaunay Triangulation from the cone positions + + return Delaunay(points) + +def visualize_triangulation(points, tri): + # Visualize and verify triangulation of points + + plt.figure(figsize=(10,8)) + + plt.triplot(points[ : , 0], points[ : , 1], tri.simplices, 'b-', linewidth=0.5) + plt.plot(points[ : , 0], points[ : , 1], 'ro', markersize=10, label="Cones") + + plt.xlabel('X position') + plt.ylabel('Y position') + plt.title('Delaunay Triangulation of Cones') + plt.legend() + plt.show() + +if __name__ == "__main__": + # Test data option 1: Simple track-like pattern + simple_track = [ + [0, 0], [1, 0], [2, 0.5], [3, 1], [4, 1.5], + [4, -0.5], [3, -1], [2, -1.5], [1, -1], [0, -0.5] + ] + + # Test data option 2: Oval/loop track (more realistic) + oval_track = [ + # Outer boundary + [0, 0], [2, -1], [4, -1.5], [6, -1.5], [8, -1], [10, 0], + [10, 2], [8, 3], [6, 3.5], [4, 3.5], [2, 3], [0, 2], + # Inner boundary + [2, 0.5], [4, 0], [6, 0], [8, 0.5], [8, 1.5], [6, 2], + [4, 2], [2, 1.5] + ] + + # Test data option 3: Slalom/chicane pattern + slalom = [ + [0, 0], [1, 1], [2, -1], [3, 1.5], [4, -1.5], + [5, 2], [6, -2], [7, 2.5], [8, -2.5], [9, 0], + [0, -0.5], [1, -2], [2, 1], [3, -2.5], [4, 2], + [5, -3], [6, 2.5], [7, -3.5], [8, 3], [9, 0.5] + ] + + # Test data option 4: Grid pattern (stress test) + grid = [] + for x in range(0, 10, 2): + for y in range(-4, 5, 2): + grid.append([x, y]) + + # Test data option 5: Random scattered (realistic chaos) + import random + random.seed(42) + random_scatter = [[random.uniform(0, 10), random.uniform(-3, 3)] for _ in range(30)] + + # Choose which test data to use + test_cone_data = random_scatter# Change this to try different patterns + + # Parse and create triangulation + points = parse_cone_data(test_cone_data) + tri = create_delaunay_triangulation(points) + + # Visualize + visualize_triangulation(points, tri) From d0929b762877cdeff662980da6fe1dd87b47984d Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Wed, 12 Nov 2025 01:27:05 -0800 Subject: [PATCH 03/18] delaunay --- PathPlanning/delaunay.py | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/PathPlanning/delaunay.py b/PathPlanning/delaunay.py index 813d330..83accaf 100644 --- a/PathPlanning/delaunay.py +++ b/PathPlanning/delaunay.py @@ -1,18 +1,17 @@ from scipy.spatial import Delaunay import numpy as np import matplotlib.pyplot as plt - -def parse_cone_data(cones): - # Parses cone data from mapping team to create points to use for triangulation - - return np.array(cones) - -def create_delaunay_triangulation(points): +def create_delaunay_triangulation(cones): # Create a Delaunay Triangulation from the cone positions - + points = np.array(cones) + return Delaunay(points) +def get_midpoint_graph(tri): + # Get a midpoint graph from the Delaunay Triangulation + return + def visualize_triangulation(points, tri): # Visualize and verify triangulation of points @@ -64,11 +63,11 @@ def visualize_triangulation(points, tri): random_scatter = [[random.uniform(0, 10), random.uniform(-3, 3)] for _ in range(30)] # Choose which test data to use - test_cone_data = random_scatter# Change this to try different patterns + test_cone_data = slalom# Change this to try different patterns # Parse and create triangulation - points = parse_cone_data(test_cone_data) - tri = create_delaunay_triangulation(points) + points = np.array(test_cone_data) + tri = create_delaunay_triangulation(test_cone_data) # Visualize visualize_triangulation(points, tri) From 6c573c7d22f51439973bb9ac08a460d2bb99fb4d Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Thu, 13 Nov 2025 15:26:39 -0800 Subject: [PATCH 04/18] updated delaunay for midpoints --- PathPlanning/delaunay.py | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/PathPlanning/delaunay.py b/PathPlanning/delaunay.py index 83accaf..7ba78f5 100644 --- a/PathPlanning/delaunay.py +++ b/PathPlanning/delaunay.py @@ -8,17 +8,32 @@ def create_delaunay_triangulation(cones): return Delaunay(points) -def get_midpoint_graph(tri): +def get_midpoint_graph(tri, points): # Get a midpoint graph from the Delaunay Triangulation - return + waypoints = [] + + for simplex in tri.simplices: + i, j ,k = simplex + # Get coordinates of each point + p1 = points[i] + p2 = points[j] + p3 = points[k] + + # Compute midpoints of each edge and add to waypoints + waypoints.append((p1 + p2) / 2) + waypoints.append((p1 + p3) / 2) + waypoints.append((p2 + p3) / 2) + + return np.array(waypoints) -def visualize_triangulation(points, tri): +def visualize_triangulation(points, tri, waypoints): # Visualize and verify triangulation of points plt.figure(figsize=(10,8)) plt.triplot(points[ : , 0], points[ : , 1], tri.simplices, 'b-', linewidth=0.5) plt.plot(points[ : , 0], points[ : , 1], 'ro', markersize=10, label="Cones") + plt.plot(waypoints[ : , 0], waypoints[ : , 1], 'go', markersize=10, label="Waypoints") plt.xlabel('X position') plt.ylabel('Y position') @@ -63,11 +78,12 @@ def visualize_triangulation(points, tri): random_scatter = [[random.uniform(0, 10), random.uniform(-3, 3)] for _ in range(30)] # Choose which test data to use - test_cone_data = slalom# Change this to try different patterns + test_cone_data = oval_track# Change this to try different patterns # Parse and create triangulation points = np.array(test_cone_data) tri = create_delaunay_triangulation(test_cone_data) + waypoints = get_midpoint_graph(tri, points) # Visualize - visualize_triangulation(points, tri) + visualize_triangulation(points, tri, waypoints) From 3ac6743ac5d778eb19d0474c9379096165b194c0 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Fri, 14 Nov 2025 13:52:22 -0800 Subject: [PATCH 05/18] Add path planning module structure MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add PathPlanning module skeleton files - Add config.py with all constants and parameters - Update PathPlanning/README.md with module overview - Create blank files for path_tree, cost_functions, beam_search, path_smoother, and path_planner Ready for implementation starting Nov 22. 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- PathPlanning/README.md | Bin 0 -> 4324 bytes PathPlanning/beam_search.py | 9 +++++++ PathPlanning/config.py | 45 +++++++++++++++++++++++++++++++++ PathPlanning/cost_functions.py | 8 ++++++ PathPlanning/path_planner.py | 13 ++++++++++ PathPlanning/path_smoother.py | 9 +++++++ PathPlanning/path_tree.py | 9 +++++++ 7 files changed, 93 insertions(+) create mode 100644 PathPlanning/beam_search.py create mode 100644 PathPlanning/config.py create mode 100644 PathPlanning/cost_functions.py create mode 100644 PathPlanning/path_planner.py create mode 100644 PathPlanning/path_smoother.py create mode 100644 PathPlanning/path_tree.py diff --git a/PathPlanning/README.md b/PathPlanning/README.md index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..3b6ef9faaf2cd2ec06945bbd10ed387dc4f9b689 100644 GIT binary patch literal 4324 zcmcInO>^5u65UHKIOmXKS5%dfX~~Xd%bTz&2T8VKS6L2AWW2R|p@zVam=b_NW(Jg* z(y!28-9NCedjJrWWtV+eT|@vqJ^k_eb@v`kRLE&ksH!s6iY{!{6go*}x$#uW1DUce zbg8T0$!S#_22*OP`B>c@TUtjU*YxMv^o-7@C$A}UX02VJy{EO#O+lnU6t{kU* z|Giz)FNe>LA0^3i_KlM~4^HdOIf32`FKA-x*0refv@u2IX#tOrn8T%Uh$$Z>yJSDS zSrie2tq~WhQY-i_u&iu`pPc>Zkq@p(ky{;=pPLH0p*qbKtGVaw(4Y4<5WI6iXSDwc z#uTQ~V`9&aj^Nje2tgm_UBdm2bApA}YgIH7y4E?{RGD%cS{vKuGXtgsFHvH)gToJ7OoOc6zCu2CCz?uW!%WP zPnq$lL*niO=u8zUqnXX?6IDCA6xx!S`8_frB-XmHDLg|yh60z`D>%*E_=R%&jCWn0 z{@FVxM7{Ox0@R0EjZtK#Y-wL?6JFa=dtq(;N!V2pvjSS_sdf79p;#nVJrBRD13?3p&OhOE9*?iOPZKkbH0+~>gp;HQ|`DMAi{Ulljt4& zt0OkN-qOz<4WY|VK&5=XqZS7-={Nmze@*b(FtHAhkyyPGnQCxI|xZv;V?r2jGuyl z#@jgWbRbClAt@fyMb~LM%8djaTYEI(XpC03+Yx#jw#j?k4HdmfLpx4CZ5s=2D9lG& zW8|7o+sPmx&&NYt;Ak5QK?d{K@KT|A-BUe1Mny*o99(&y^9Gl_7)QiSbrD+u{X`LT z(d_nafhH5?fOQ5bXwfR=)w9;tfEL9V2QY;-wuudM82@8QwG=B8gw*oZ^Y*;l|9H!8 zei(=k_ZW@RmJcJIIxI*KGZmB!9Bk2$Af&u6n>dutP>z`QUZY`mGbsQ|JlAEa;L!$L zi>?7ujwy!rgcD_VeUugAg?3+3}7=NHSP8@`UZ7;a%T$Zb;^VqJN36H z+->l^Hu2R<3xuQJV4(Qh9YmMyH7*-;6Gp7`^o;z*2VH)%>TQqfh>eD$`=$P?F)r>g z3Gg}^ojbh30EE?H+>+@hJSu{3l*?|E(=2zXe7(>P)7!+z$IEUd;;$DCz!b+7%moXL zSz1%2KVpjH?%31-JhR4VbZHlm<-v{5y#bOz-IX);h2$d?A>J+TAV}Vk}Cto4DE6KgDhAdO<=c14cAF zk^yGHuC4Z)sNCC`eW^C|AKM(zd(@1=xT4(37KUjDY5bybjJl`&kR^Qcb;f}c_*l)0 zFU`sk zC+Ze34Iy4Wo*h3ZJ%O|L+h=>?=m8Uo~Zih*9Qfqy`8sPt%ryVnGcGJ z6|qzB{Q4!)c9;@i$FVU=zkEP37XSkSNpE5BZ|T6(29PjxvQq9w75I9hGhpOYS88pr H2qymn1hjid literal 0 HcmV?d00001 diff --git a/PathPlanning/beam_search.py b/PathPlanning/beam_search.py new file mode 100644 index 0000000..2741bab --- /dev/null +++ b/PathPlanning/beam_search.py @@ -0,0 +1,9 @@ +""" +Beam Search Path Pruning +Deadline: Nov 29, 2025 +""" + +import numpy as np +from typing import List +from path_tree import PathNode +import config diff --git a/PathPlanning/config.py b/PathPlanning/config.py new file mode 100644 index 0000000..7142bcd --- /dev/null +++ b/PathPlanning/config.py @@ -0,0 +1,45 @@ +""" +Path Planning Configuration +Contains all constants and parameters for the path planning system +""" + +# Performance Requirements +PLANNING_FREQUENCY_HZ = 25 # Hz - must compute new path 25 times/sec +CYCLE_TIME_MS = 40 # ms - max computation time (1000/25) + +# Track Constraints (from Formula Student Driverless rules) +MIN_TRACK_WIDTH = 3.0 # meters (DD.2.2.2.c) +MIN_TURNING_DIAMETER = 9.0 # meters (DD.2.2.2.d) +MIN_TURNING_RADIUS = MIN_TURNING_DIAMETER / 2 # 4.5 meters +MAX_STRAIGHT_LENGTH = 80.0 # meters (DD.2.2.2.a) +LAP_LENGTH_MIN = 200.0 # meters (DD.2.2.2.e) +LAP_LENGTH_MAX = 500.0 # meters (DD.2.2.2.e) + +# Cone Colors (from DD.1.1) +CONE_COLOR_BLUE = 0 # Left track border +CONE_COLOR_YELLOW = 1 # Right track border +CONE_COLOR_ORANGE_SMALL = 2 # Entry/exit lanes +CONE_COLOR_ORANGE_LARGE = 3 # Start/finish lines + +# Beam Search Parameters +BEAM_WIDTH = 10 # Number of best paths to keep at each tree level +MAX_TREE_DEPTH = 15 # Maximum number of waypoints to look ahead + +# Cost Function Weights +WEIGHT_BOUNDARY_VIOLATION = 1000.0 # Heavily penalize leaving track +WEIGHT_TURNING_RADIUS = 100.0 # Penalize sharp turns +WEIGHT_CURVATURE_CHANGE = 50.0 # Prefer smooth paths +WEIGHT_DISTANCE = 1.0 # Prefer shorter paths +WEIGHT_CENTER_LINE = 10.0 # Prefer staying near center of track + +# Path Smoothing Parameters +SPLINE_DEGREE = 3 # Cubic spline +SPLINE_SMOOTHING_FACTOR = 0.1 # Lower = smoother, higher = closer to waypoints + +# Vehicle Dynamics (estimated - coordinate with Control Planning team) +VEHICLE_MAX_SPEED = 20.0 # m/s (72 km/h - from competitor data) +VEHICLE_MAX_ACCELERATION = 15.0 # m/s^2 (0-100 km/h in ~2s) + +# Noise/Robustness Parameters +MAX_CONE_POSITION_ERROR = 0.5 # meters - expected SLAM noise from monocular vision +MIN_CONES_FOR_VALID_PATH = 4 # Minimum cones needed to plan a path diff --git a/PathPlanning/cost_functions.py b/PathPlanning/cost_functions.py new file mode 100644 index 0000000..66ef316 --- /dev/null +++ b/PathPlanning/cost_functions.py @@ -0,0 +1,8 @@ +""" +Cost Function Heuristics for Path Planning +Deadline: Dec 3, 2025 +""" + +import numpy as np +from typing import List, Tuple +import config diff --git a/PathPlanning/path_planner.py b/PathPlanning/path_planner.py new file mode 100644 index 0000000..a4828b8 --- /dev/null +++ b/PathPlanning/path_planner.py @@ -0,0 +1,13 @@ +""" +Main Path Planning Module +Deadline: Dec 6, 2025 +""" + +import numpy as np +from typing import Tuple +from delaunay import create_delaunay_triangulation, get_midpoint_graph +from path_tree import PathTree +from cost_functions import PathCostEvaluator +from beam_search import BeamSearchPathPlanner +from path_smoother import PathSmoother +import config diff --git a/PathPlanning/path_smoother.py b/PathPlanning/path_smoother.py new file mode 100644 index 0000000..f98ed55 --- /dev/null +++ b/PathPlanning/path_smoother.py @@ -0,0 +1,9 @@ +""" +Path Smoothing using Spline Interpolation +Deadline: Dec 3, 2025 +""" + +import numpy as np +from typing import List, Tuple +from scipy.interpolate import splprep, splev, CubicSpline +import config diff --git a/PathPlanning/path_tree.py b/PathPlanning/path_tree.py new file mode 100644 index 0000000..524ff6d --- /dev/null +++ b/PathPlanning/path_tree.py @@ -0,0 +1,9 @@ +""" +Path Tree Population Module +Deadline: Nov 22, 2025 +""" + +import numpy as np +from typing import List, Optional +from dataclasses import dataclass +import config From 31812f3c00e9d4af7b4cd96b7f02bd953276a86d Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Fri, 14 Nov 2025 14:14:56 -0800 Subject: [PATCH 06/18] update README --- PathPlanning/README.md | Bin 4324 -> 11831 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/PathPlanning/README.md b/PathPlanning/README.md index 3b6ef9faaf2cd2ec06945bbd10ed387dc4f9b689..e02bbf5cf0f1476894f84d0591df4382046ae69d 100644 GIT binary patch literal 11831 zcmcIq?QR>#lKs!8sB2;GQZy-9vY9x-`T`PV%R(fX5ar}1K|l{@n&e!~m*+z=10&C~ zzxNS#Uu@5*?wKJe*+~{y13^TxySlnQPgV7fjJrFj>fD8dQ<#bZ(YmA+dgt zFwPxNB(*}kQVxgm_P4!Gr`J323KK}0^m<2feEGASwgkJcZ{Et!RUZ1w zU74j-5_tJaOhJ-mm?q*yi!=|*W!xD^uQ&EGlatJoO*%(%1D4-ZQ6$GznI>tRR)t(! zjwY+3H1QMZe(%kG^Aa&7sP6UU@Y%t4yGnMH#+fNYJ@DETRa6!{q?EQDS=or66H*H4pqxY)~9y`Bv4Ge1>S zGS!d-g5QhMgY9%gu;2y?zqMcHUPzPb#$h7&VE}V_9`~iJay7)<3qok)#ojR9wOhxn z#9%ME7aK?h`_*LnGBaLm>A3Zy6oQ;riIu{=2^UKq_tEdm$9w!&;6Fc&(j5Pf+@2q- zsY5lB#FQRw67h6F?mLM9I(CKD{MrdWD8TzR&VL&qO>)&dWU%8SC^tZ=F_ zjKClEbm7Tal$KFAb1Qp2dM}u;u}C+N6G$#lNeI^F2n2n(H&PWwRGNC1f*~yQ!*XS% z-Rn(CudLKx&l4Hv=^|&O>RzRH@^Wx6gi@bVsf8y>X>2g24BcH&1#)?Pau65 z@Ze3D7p3F(qza>;keT~w&Yy67>`dp>LlMp*8%CkEWu8`xWg|nY24``lmutvO(!d;v zU*+)nvJR?^M(yHDFYzdHvU!E|lFt5xFi^lT7_Om*q}=zH;0&SMT4b>dv)UXypNp^( zHk0VSG8G?tOpb%pUBP?-78rCHxK+dU?29lCOXx!H7?=!Xusb%0oG+fmh4?EZ0ny~4 zK|%e$)(3U#Wm%qr1!}dHs1+N+B;9l#egb_Q;^Irh6Ke{sMz7LR@F^!V-BV~L3KK|< zx;;FEW?$I2Rg}{@Iusk~)xh!k&WkE*mEEBxpc|9NFk7D2SRqws#C(Di=^B4C=1}NE zm?J|EB|{2FN`ZNa1m%FYz4ShXaTOzAE+8U*snKO-%6nrHtB}GhGz0T=#?fnK&5Lla z3EV4##&RUrLJdDgE(+l%>iP(CMHK`fq|~t@iNaeGtv2~MBp)_9+hEM7jl3F2VD3UE zh~J;bxNHW?FoBx+7Ax1l4)Av@2}BEB2dyQv&SesXu{l?Bw8V(r?6p%AP{yoMe>4N{Frlz<_Up$&b%2z z)JnRus4}l2GcOTXO%$c~uX3vSF|lZHZ#MJFj-Z1q;7}tqV2TgFL7J%5d}5HC@$+vc zPbHj70@*QTNth^luxa+r3nLHFWRv|N+1Gd#nPdS=Y!#`e6_2E{ht@SV3F9*yH%$@A zkpcEqXbclDtt=-Dkquh*IAx~SQ)hZpXW=)1Jcyd$j5qx7;X}46m-f2qIXo7A36y?!ks)V*?_QnK~o;(he)} zb*}Dd6jS|vO1(ZfB-%KHvYJ|{OoMPf$8vXlr?J-zyo?U*%>%gY$BK!wuQ;j>BHTET zAO8T&07fM=bNFfww4RjeW4d)c_kAH!o*G}CBhn2SD2c14h4FHKSx3neLam8@h1^*Y zuUWSE0oJN1MAHC#{nnTaFdp7krrF>YIE?_A) zd)O~ik3E#M?Rrf*$SuI)6;e7}2kZGMGN0(CL{4i(OA8_G_H(2*KprBe&CqR{@NXAd z^Z}57&`Q60#|ypKJN!4z=6V~(0wg~eU>}uJQ8K=i@{ez02||W1-?98B6riaJWk%DJ9K%S*Gm0MOPx)RVVEc_f8SsEmVP zLX3>y*4aM(Z|knI|i+fAsrJc2s>(4t0a8y;QeX|#2Q)!#b8cw-FU znGMncepHN-xJKr{%(;~T5+$G8$RU7S&6@e?c)j746X;_yi zO*83kp(lqg*Gw1A4VaNFQ85>^$F}@cqLYi`OC$^!%A92>;5mF{_XvLB8x-j@eOOXc z8Z>3NQ|p#I*D0k{X-OgeLzwFKfmeE4B(B@8LF<6gp(CgMTPcDFlDi1snhVu`ZwZVT=G_s|I{nkDmGw@lY4wgY_q|F3g?4FSKUd@-uY# z|NieY4thHH6aQ?ZMlK#TQ2Ap5PgR22QW(>2<={xpwDxq-fSv*HZfokz8rrsB&S}N8 z6NqTtdR?Wf5QiGl-p7S44Y~aB8e6)oKu|RnuBz6?QJ(rJ3$Y7gLAPUsOoDjYaJ5-% ztK&s-7uw2>PQ|(k%>mm1U=#$Lfqer_!N?Wyt6MT>S#XZ^s5@O*-jsvRsiUTPZpto{ zGPV+bv=Fy2&sN`hP*anuP~NHcQYM#m`Cq3BSAbFo+m+d=O)I!WA^#lwWDzxk0->o^ zjjVLE4S^fmu8~#oAq$MJRn87mZ0Wc4q^Do5ejihp)_@jSyB1^<<7sBn>T4QRwX2%V;b+v`MJx!TQz}^1_vX z6Eu%)BlPbsyD;z4T$(s~QUmkGf?Zi^m13L7+e)!?tv0mB&=Y6A_wR%87dL!+C~Z>7heg2XZbOyhNHdd>W%p1f>{c1+x&fbLObP4gM0X^u`H z^CBhAY;p~ze@?5uTwyy#CzoEnNF&UX5Dx4;#`WBY7qXb_y0QbP8(1N-hE~LG{sep= zw*Q090~!%rL0}l-e;?2knNHEHH`A%SdL>V$(-;7II(_m%W~er9(QX4)n|Z~uvm+$_ zrS#n$y}!PGeRXs3ll=JZ`up>%v)zvEPAEF7S>6xYK0h4Il2nyz6zh6(1e11TrV#&e zLflmMmK#bNK@1Z6^W?a5qoLW*Tn|EjfgrcFg?~d$@9N5?{I4*m_pLXgjBO~+Ie{ZS z(Mmj!LPu|ZnRHucwb-(OYO49MS2E|qyg2w*uB(Mgqi)Tq)i#8VI-3OKm86L7um(=Y^xuxK zYC_`xWjBATYaTz$eUzpT3FwT&v?O)}d6zXih_W#`DANIlYnUjm05={X5Fy0IZHa+l zALh1RV)hIBixb1cor@fKY3<(*+V99-h^&;J2IAas0+s_;hQUJuKu19eW5Xbc-XplY ze#1ZoZDX~teh=BQcg3By+!K2 zN4L$!uOge793$m%C|w;g0Md@e@AqR9r}@fe@TRC$01e*2EZa^9gfjzy1nM8!mTn?ao8cPE*voIZPiy0~fy!o?415%# zjTU@mg6xL5>s_-G#gLR+<24%v&XN2+9AMyysAMN=i0GVGQyhvkXPkHeRvgI>&K;ce zTJ(zf35UcGR@-1(qk9L8f{p;{BQ=_ixEd#$n&k#^*Jvufu%wnKA@o~@m=J46J1bUL5qSbI~b z%&DJ^M+u+hU6pT>`P}(j7skyW^%sM|BVP@$W3%){S)emcoXO!YGV0T(yZG>>qIRt9 zb*uoP_iBXFxCFMT8Fdez!7p~(J_~+O>DbBou9KS5sNDoh&XWK}kFXcXcqC8dZ2TVI zu8R}g;XAf|EdxLwn4K3`brGH(Ut`Y%-fmJEA${i_zQ1fdz^x_UH4Wng5k0n)K=JG?wc?MIn6 z%TG;#d>E|N1kKE)=~6V+21|;yy`!dwUl+Uemd$klRr+3F4hA9CeGIL1K#J!%5bsxI zi$ud)XB<$;4dzs~6^oeX6}NEFdUlUL(aXVm4aOiFG1sDH^C#m(le%uS1I2lSb`mu} z((Uzy2mX)3w|$0#w@ci7L%8yH`N6$OVSwTCgW(s}>(w>&tvv>@dkznEbNdj1^firb z&duBrhu=N2T=Qf-Iz`H}&kztY@eBdWb<*(2Hs#9JqKuXKv{FeH|wiZcwr5EZkI?L)4=ZDN7MRgJnD^hm<4>C5EOJe*o5 z!dk%&y-ZCA*W=6zft5omgtK-!ML4N4X>dWuo!-7NLOH_MM&vh(%7z*JW2Z3)wIz!@ zuMiEGfQc{ZVtTHHO%^>6w6QtWA}|UH(I#Zha*jZt=ATtYhh;XyN(Bo?Xdk7+U8(@ z?+y9dAr1@Z3SB=-Qo2i2G{|MIa9xYMv5N0ia|8Y(eE)23So5#CfA9ptF~>O$vb<9O zqAue9g4F_FHzMf3Miq*(`)Rkh+wjq!#tUd*qx3@IvR=$(bl!zCdUmWpE!_FbZPqD>AgOYi6M*@M__U z0-m}ds33+|gcXMC4E=;@K=%S&az*bIAV?_33opO-A}l&K0RYnp9-t_`QUcAZR}}Gh z*^9>wc=1Pd4P@h??*HYjuYKGLN`Ex|Lx&D7*GyTJdDmSux`_gKeQLYe>2cYhMfpaw#Qg{%y0;Tbwq6aMkQ{u?>s)i`;Fr{4933pDjPeV2VU7nTsyD#TgA@2? zFtawU;ML|Ecy=g<4~DX1aUNHBz=%dma%hAnjNsO_fY-LnjK?lz0wlJKCC>Axl$Ng~ zWf9=lEon>>F$<~lw`;XpvU2J449r7`%mJ-uxW_`tI<&pfBa-$}fmO{p?WZ|wL4`Bz zG^0PtJ2KBh#U{m}60rmG_-5-J=u$%4*Zlv(sco6u|IIzi2zIvR>d69>r5~DNlrQ)y zpyH`bP}hobqEg6XRx5`(Q(UgaXu)J7jJr-AS3(m7nGtRzl_kx@3d7p=%*|z}WgZSf zn>U3+BI_ad!%dS0UDGVog(gajC2KAeo=bh|!>pp8*zqu#%;j=0+hhTUW5s?-mvJLf zZJ2(LlA_qW&Ac#{@Iew(Xy>gTVwIr!!HySgv+(=ySZR^T>7MYGSP1$77kf%mz_q_P zI7?W#FfxLf(Oe>mg%?J5A6{brI`@<#OotLwbwQjt%yU-v0Mlbxd^+|7KcshjoZR(( zEq=+=@Wglv-{n3b+n2`d#A8~Y4R_(PYQq$CfgTZQo7~XUW!#w0Ze4LgitFN$#LmI^ z#^fYj`Hi>oyL$(xqJtNCIN2qMIB?AVQ*;9r!!$iiHbN}vSjl$K`dsmEzA&-+0`(KB UG#Et9WqJ&lgqxh>^Fw?80nfsw{{R30 From 7fed23ad9679241c181018ebe79e772839194123 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Fri, 14 Nov 2025 14:17:42 -0800 Subject: [PATCH 07/18] update README --- PathPlanning/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/README.md b/PathPlanning/README.md index e02bbf5..8d924c7 100644 --- a/PathPlanning/README.md +++ b/PathPlanning/README.md @@ -289,7 +289,7 @@ ql = 0.5 # Path length deviation weight ## Team -- Nathan Yee (Lead) +- Nathan Yee - Suhani Agarwal - Aedan Benavides From 057438cf3d9d79f1a3652953e6b01b435bfed27f Mon Sep 17 00:00:00 2001 From: DanielRhee Date: Mon, 17 Nov 2025 13:57:21 -0800 Subject: [PATCH 08/18] Depth estimation documentation: --- .gitignore | 2 ++ DepthEstimation/README.md | 14 ++++++++++++++ 2 files changed, 16 insertions(+) create mode 100644 DepthEstimation/README.md diff --git a/.gitignore b/.gitignore index bec4f9f..84609f9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,7 @@ .DS_Store /Data/fsoco_segmentation_train /Segmentation/Yolact_minimal +/DepthEstimation/TestData +/DepthEstimation/Lite-Mono /SampleData/driverless.mp4 *.txt diff --git a/DepthEstimation/README.md b/DepthEstimation/README.md new file mode 100644 index 0000000..995d80d --- /dev/null +++ b/DepthEstimation/README.md @@ -0,0 +1,14 @@ +Clone +``` +git@github.com:noahzn/Lite-Mono.git +``` +into this folder and run +``` +python main.py +``` +in order to get depth estimation + +A good test video source for /TestData/ is +``` +https://www.youtube.com/watch?v=o5vES5QaeiQ +``` From b5c870c32c590eb1bb97819c1e5b0b4735d4751f Mon Sep 17 00:00:00 2001 From: DanielRhee Date: Mon, 17 Nov 2025 20:34:38 -0800 Subject: [PATCH 09/18] LiteMono test script --- .gitignore | 3 + DepthEstimation/main.py | 171 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 DepthEstimation/main.py diff --git a/.gitignore b/.gitignore index 84609f9..af03091 100644 --- a/.gitignore +++ b/.gitignore @@ -3,5 +3,8 @@ /Segmentation/Yolact_minimal /DepthEstimation/TestData /DepthEstimation/Lite-Mono +/DepthEstimation/output +/DepthEstimation/output_best +/DepthEstimation/weights /SampleData/driverless.mp4 *.txt diff --git a/DepthEstimation/main.py b/DepthEstimation/main.py new file mode 100644 index 0000000..53f7675 --- /dev/null +++ b/DepthEstimation/main.py @@ -0,0 +1,171 @@ +import os +import sys +import argparse +import numpy as np +import cv2 +import torch +from torchvision import transforms +import matplotlib.cm as cm +import matplotlib.colors as colors +from PIL import Image + +sys.path.append('./Lite-Mono') +import networks +from layers import disp_to_depth + +MODEL_CONFIGS = { + 'tiny': { + 'name': 'lite-mono-tiny', + 'folder': 'lite-mono-tiny_640x192', + 'description': 'Lightest/fastest (2.2M params, 640x192)' + }, + 'best': { + 'name': 'lite-mono-8m', + 'folder': 'lite-mono-8m_1024x320', + 'description': 'Best quality/heaviest (8.7M params, 1024x320)' + } +} + + +class DepthEstimator: + def __init__(self, weightsFolder, model="lite-mono-tiny", useCuda=True): + self.device = torch.device("cuda" if torch.cuda.is_available() and useCuda else "cpu") + + encoderPath = os.path.join(weightsFolder, "encoder.pth") + decoderPath = os.path.join(weightsFolder, "depth.pth") + + encoderDict = torch.load(encoderPath, map_location=self.device) + decoderDict = torch.load(decoderPath, map_location=self.device) + + self.feedHeight = encoderDict['height'] + self.feedWidth = encoderDict['width'] + + print(f"Loading {model} model ({self.feedWidth}x{self.feedHeight}) on {self.device}") + + self.encoder = networks.LiteMono(model=model, height=self.feedHeight, width=self.feedWidth) + modelDict = self.encoder.state_dict() + self.encoder.load_state_dict({k: v for k, v in encoderDict.items() if k in modelDict}) + self.encoder.to(self.device) + self.encoder.eval() + + self.depthDecoder = networks.DepthDecoder(self.encoder.num_ch_enc, scales=range(3)) + depthModelDict = self.depthDecoder.state_dict() + self.depthDecoder.load_state_dict({k: v for k, v in decoderDict.items() if k in depthModelDict}) + self.depthDecoder.to(self.device) + self.depthDecoder.eval() + + self.toTensor = transforms.ToTensor() + + def processFrame(self, frame): + originalHeight, originalWidth = frame.shape[:2] + + frameRgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + framePil = Image.fromarray(frameRgb) + frameResized = framePil.resize((self.feedWidth, self.feedHeight), Image.LANCZOS) + + inputTensor = self.toTensor(frameResized).unsqueeze(0).to(self.device) + + with torch.no_grad(): + features = self.encoder(inputTensor) + outputs = self.depthDecoder(features) + + disp = outputs[("disp", 0)] + dispResized = torch.nn.functional.interpolate( + disp, (originalHeight, originalWidth), mode="bilinear", align_corners=False) + + dispNp = dispResized.squeeze().cpu().numpy() + + vmax = np.percentile(dispNp, 95) + normalizer = colors.Normalize(vmin=dispNp.min(), vmax=vmax) + mapper = cm.ScalarMappable(norm=normalizer, cmap='magma') + colormapped = (mapper.to_rgba(dispNp)[:, :, :3] * 255).astype(np.uint8) + + return colormapped, dispNp + + def processVideo(self, videoPath, outputFolder="output", saveFrames=True, displayLive=False): + os.makedirs(outputFolder, exist_ok=True) + + cap = cv2.VideoCapture(videoPath) + if not cap.isOpened(): + raise ValueError(f"Cannot open video: {videoPath}") + + fps = cap.get(cv2.CAP_PROP_FPS) + frameCount = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + + print(f"Processing video: {frameCount} frames at {fps:.2f} FPS ({width}x{height})") + + outputVideoPath = os.path.join(outputFolder, "depth_output.mp4") + fourcc = cv2.VideoWriter_fourcc(*'mp4v') + outVideo = cv2.VideoWriter(outputVideoPath, fourcc, fps, (width, height)) + + frameIdx = 0 + while True: + ret, frame = cap.read() + if not ret: + break + + depthColor, depthRaw = self.processFrame(frame) + depthBgr = cv2.cvtColor(depthColor, cv2.COLOR_RGB2BGR) + + outVideo.write(depthBgr) + + if saveFrames and frameIdx % 30 == 0: + cv2.imwrite(os.path.join(outputFolder, f"frame_{frameIdx:06d}_depth.png"), depthBgr) + np.save(os.path.join(outputFolder, f"frame_{frameIdx:06d}_depth.npy"), depthRaw) + + if displayLive: + combined = np.hstack((frame, depthBgr)) + cv2.imshow('Original | Depth', combined) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + frameIdx += 1 + if frameIdx % 30 == 0: + print(f"Processed {frameIdx}/{frameCount} frames ({100*frameIdx/frameCount:.1f}%)") + + cap.release() + outVideo.release() + if displayLive: + cv2.destroyAllWindows() + + print(f"\nComplete! Processed {frameIdx} frames") + print(f"Output video: {outputVideoPath}") + return outputVideoPath + + +def main(): + parser = argparse.ArgumentParser(description='Lite-Mono Depth Estimation') + parser.add_argument('--model', type=str, default='tiny', choices=['tiny', 'best'], + help='Model to use: tiny (fastest) or best (highest quality)') + parser.add_argument('--video', type=str, default='./TestData/test1.mp4', + help='Path to input video') + parser.add_argument('--output', type=str, default=None, + help='Output folder (default: ./output_)') + parser.add_argument('--cuda', action='store_true', default=True, + help='Use CUDA if available') + + args = parser.parse_args() + + config = MODEL_CONFIGS[args.model] + weightsFolder = f"./weights/{config['folder']}" + outputFolder = args.output or f"./output_{args.model}" + + print(f"Using model: {config['description']}") + + if not os.path.exists(weightsFolder): + print(f"Error: Weights folder not found: {weightsFolder}") + print("Please download the model weights first") + return + + if not os.path.exists(args.video): + print(f"Error: Video not found: {args.video}") + return + + estimator = DepthEstimator(weightsFolder, model=config['name'], useCuda=args.cuda) + estimator.processVideo(args.video, outputFolder=outputFolder, saveFrames=True, displayLive=False) + + +if __name__ == '__main__': + main() From 7ec93f45e7063711111d7e4b4378ef57ebe319e8 Mon Sep 17 00:00:00 2001 From: DanielRhee Date: Mon, 17 Nov 2025 20:35:38 -0800 Subject: [PATCH 10/18] archive it --- DepthEstimation/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/DepthEstimation/README.md b/DepthEstimation/README.md index 995d80d..d01f7d4 100644 --- a/DepthEstimation/README.md +++ b/DepthEstimation/README.md @@ -1,3 +1,5 @@ +# Lite Mono archive +The model was deemed 'not good enough' to continue use. It is too slow, and just not good enough at large depths Clone ``` git@github.com:noahzn/Lite-Mono.git @@ -12,3 +14,4 @@ A good test video source for /TestData/ is ``` https://www.youtube.com/watch?v=o5vES5QaeiQ ``` + From e7e9031a7ddc9bd20be7a2011113f0d201869aa3 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Thu, 20 Nov 2025 21:10:31 -0800 Subject: [PATCH 11/18] initial version of delaunay.py --- PathPlanning/delaunay.py | 144 +++++++++++++++++++++++++++++++-------- environment.yml | 4 +- 2 files changed, 117 insertions(+), 31 deletions(-) diff --git a/PathPlanning/delaunay.py b/PathPlanning/delaunay.py index 7ba78f5..b7a6699 100644 --- a/PathPlanning/delaunay.py +++ b/PathPlanning/delaunay.py @@ -1,45 +1,121 @@ from scipy.spatial import Delaunay +from collections import defaultdict import numpy as np import matplotlib.pyplot as plt +import time -def create_delaunay_triangulation(cones): - # Create a Delaunay Triangulation from the cone positions - points = np.array(cones) - - return Delaunay(points) - -def get_midpoint_graph(tri, points): +def get_midpoints(cones): # Get a midpoint graph from the Delaunay Triangulation - waypoints = [] + points = np.array(cones) + tri = Delaunay(points) + waypoint_graph = defaultdict(set) + + # Vectorize this for speedup instead of using a loop + # Get coordinates of each point + p1 = points[tri.simplices[:,0]] + p2 = points[tri.simplices[:,1]] + p3 = points[tri.simplices[:,2]] + + # Compute midpoints of each edge and store + wayp1p2 = ((p1 + p2) / 2) + wayp1p3 = ((p1 + p3) / 2) + wayp2p3 = ((p2 + p3) / 2) - for simplex in tri.simplices: - i, j ,k = simplex - # Get coordinates of each point - p1 = points[i] - p2 = points[j] - p3 = points[k] + for i in range(len(wayp1p2)): + wp1, wp2, wp3 = wayp1p2[i], wayp1p3[i], wayp2p3[i] + # Convert to tuples (hashable for dict keys) + wp1_tuple = tuple(np.round(wp1, decimals=6)) + wp2_tuple = tuple(np.round(wp2, decimals=6)) + wp3_tuple = tuple(np.round(wp3, decimals=6)) + # Each waypoint connects to the other 2 in the triangle + waypoint_graph[wp1_tuple].add(wp2_tuple) + waypoint_graph[wp1_tuple].add(wp3_tuple) + waypoint_graph[wp2_tuple].add(wp1_tuple) + waypoint_graph[wp2_tuple].add(wp3_tuple) + waypoint_graph[wp3_tuple].add(wp1_tuple) + waypoint_graph[wp3_tuple].add(wp2_tuple) + + # Stack all the waypoint arrays into one vertically + waypoints = np.unique(np.vstack([wayp1p2, wayp1p3, wayp2p3]), axis=0) - # Compute midpoints of each edge and add to waypoints - waypoints.append((p1 + p2) / 2) - waypoints.append((p1 + p3) / 2) - waypoints.append((p2 + p3) / 2) - - return np.array(waypoints) + return waypoints, waypoint_graph, tri def visualize_triangulation(points, tri, waypoints): # Visualize and verify triangulation of points plt.figure(figsize=(10,8)) - + plt.triplot(points[ : , 0], points[ : , 1], tri.simplices, 'b-', linewidth=0.5) plt.plot(points[ : , 0], points[ : , 1], 'ro', markersize=10, label="Cones") plt.plot(waypoints[ : , 0], waypoints[ : , 1], 'go', markersize=10, label="Waypoints") - + plt.xlabel('X position') plt.ylabel('Y position') plt.title('Delaunay Triangulation of Cones') plt.legend() plt.show() + +def visualize_waypoint_graph(waypoint_graph, points): + # Visualize the waypoint graph connections in a separate window + + plt.figure(figsize=(10,8)) + + # Draw all connections between waypoints + for waypoint, neighbors in waypoint_graph.items(): + wx, wy = waypoint + for neighbor in neighbors: + nx, ny = neighbor + # Draw line between connected waypoints + plt.plot([wx, nx], [wy, ny], 'g-', linewidth=1, alpha=0.6) + + # Plot waypoints as nodes + waypoint_coords = np.array(list(waypoint_graph.keys())) + plt.plot(waypoint_coords[:, 0], waypoint_coords[:, 1], 'go', + markersize=8, label="Waypoints", zorder=3) + + plt.xlabel('X position') + plt.ylabel('Y position') + plt.title('Waypoint Graph Connections') + plt.legend() + plt.grid(True, alpha=0.3) + plt.show() + +def visualize_combined(points, tri, waypoints, waypoint_graph): + # Visualize both triangulation and waypoint graph side-by-side + + fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(18, 8)) + + # Left plot: Delaunay Triangulation + ax1.triplot(points[:, 0], points[:, 1], tri.simplices, 'b-', linewidth=0.5) + ax1.plot(points[:, 0], points[:, 1], 'ro', markersize=10, label="Cones") + ax1.plot(waypoints[:, 0], waypoints[:, 1], 'go', markersize=10, label="Waypoints") + ax1.set_xlabel('X position') + ax1.set_ylabel('Y position') + ax1.set_title('Delaunay Triangulation of Cones') + ax1.legend() + ax1.grid(True, alpha=0.3) + + # Right plot: Waypoint Graph + # Draw all connections between waypoints + for waypoint, neighbors in waypoint_graph.items(): + wx, wy = waypoint + for neighbor in neighbors: + nx, ny = neighbor + ax2.plot([wx, nx], [wy, ny], 'g-', linewidth=1, alpha=0.6) + + # Plot waypoints as nodes + waypoint_coords = np.array(list(waypoint_graph.keys())) + ax2.plot(waypoint_coords[:, 0], waypoint_coords[:, 1], 'go', + markersize=8, label="Waypoints", zorder=3) + + ax2.set_xlabel('X position') + ax2.set_ylabel('Y position') + ax2.set_title('Waypoint Graph Connections') + ax2.legend() + ax2.grid(True, alpha=0.3) + + plt.tight_layout() + plt.show() if __name__ == "__main__": # Test data option 1: Simple track-like pattern @@ -78,12 +154,22 @@ def visualize_triangulation(points, tri, waypoints): random_scatter = [[random.uniform(0, 10), random.uniform(-3, 3)] for _ in range(30)] # Choose which test data to use - test_cone_data = oval_track# Change this to try different patterns + test_cases = [ + ("Simple Track", simple_track), + ("Slalom", slalom), + ("Grid", grid), + ("Random 30", random_scatter), + ("Oval Track", oval_track) + ] + + for name, cone_data in test_cases: + points = np.array(cone_data) + + start = time.perf_counter() + waypoints, waypoint_graph, tri = get_midpoints(cone_data) + elapsed = time.perf_counter() - start - # Parse and create triangulation - points = np.array(test_cone_data) - tri = create_delaunay_triangulation(test_cone_data) - waypoints = get_midpoint_graph(tri, points) + print(f"{name:15} | Cones: {len(points):3} | Time: {elapsed*1000:6.2f} ms | Hz: {1/elapsed:6.1f}") - # Visualize - visualize_triangulation(points, tri, waypoints) + # Visualize the last test case - both plots side-by-side + visualize_combined(points, tri, waypoints, waypoint_graph) diff --git a/environment.yml b/environment.yml index f048d15..f9178ff 100644 --- a/environment.yml +++ b/environment.yml @@ -116,7 +116,7 @@ dependencies: - pygame=2.6.1 - pyparsing=3.1.2 - pysocks=1.7.1 - - python=3.10.9 + - python=3.12.2 - python-dateutil=2.9.0post0 - python-tzdata=2023.3 - python_abi=3.12 @@ -236,4 +236,4 @@ dependencies: - watchfiles==1.1.0 - websockets==13.1 - zstd==1.5.7.2 -prefix: /Users/username/anaconda3/envs/fsae +prefix: /Users/daniel/anaconda3/envs/fsae From 845563bc42e7e65c326170dfb737a32b1f53dd41 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Mon, 24 Nov 2025 01:58:05 -0800 Subject: [PATCH 12/18] updated delaunay.py --- PathPlanning/delaunay.py | 112 +++++++++++++-------------------------- 1 file changed, 36 insertions(+), 76 deletions(-) diff --git a/PathPlanning/delaunay.py b/PathPlanning/delaunay.py index b7a6699..8d7343d 100644 --- a/PathPlanning/delaunay.py +++ b/PathPlanning/delaunay.py @@ -40,79 +40,39 @@ def get_midpoints(cones): return waypoints, waypoint_graph, tri -def visualize_triangulation(points, tri, waypoints): - # Visualize and verify triangulation of points - - plt.figure(figsize=(10,8)) - - plt.triplot(points[ : , 0], points[ : , 1], tri.simplices, 'b-', linewidth=0.5) - plt.plot(points[ : , 0], points[ : , 1], 'ro', markersize=10, label="Cones") - plt.plot(waypoints[ : , 0], waypoints[ : , 1], 'go', markersize=10, label="Waypoints") - - plt.xlabel('X position') - plt.ylabel('Y position') - plt.title('Delaunay Triangulation of Cones') - plt.legend() - plt.show() - -def visualize_waypoint_graph(waypoint_graph, points): - # Visualize the waypoint graph connections in a separate window - - plt.figure(figsize=(10,8)) - - # Draw all connections between waypoints - for waypoint, neighbors in waypoint_graph.items(): - wx, wy = waypoint - for neighbor in neighbors: - nx, ny = neighbor - # Draw line between connected waypoints - plt.plot([wx, nx], [wy, ny], 'g-', linewidth=1, alpha=0.6) - - # Plot waypoints as nodes - waypoint_coords = np.array(list(waypoint_graph.keys())) - plt.plot(waypoint_coords[:, 0], waypoint_coords[:, 1], 'go', - markersize=8, label="Waypoints", zorder=3) - - plt.xlabel('X position') - plt.ylabel('Y position') - plt.title('Waypoint Graph Connections') - plt.legend() - plt.grid(True, alpha=0.3) - plt.show() - -def visualize_combined(points, tri, waypoints, waypoint_graph): - # Visualize both triangulation and waypoint graph side-by-side - - fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(18, 8)) - - # Left plot: Delaunay Triangulation - ax1.triplot(points[:, 0], points[:, 1], tri.simplices, 'b-', linewidth=0.5) - ax1.plot(points[:, 0], points[:, 1], 'ro', markersize=10, label="Cones") - ax1.plot(waypoints[:, 0], waypoints[:, 1], 'go', markersize=10, label="Waypoints") - ax1.set_xlabel('X position') - ax1.set_ylabel('Y position') - ax1.set_title('Delaunay Triangulation of Cones') - ax1.legend() - ax1.grid(True, alpha=0.3) - - # Right plot: Waypoint Graph - # Draw all connections between waypoints - for waypoint, neighbors in waypoint_graph.items(): - wx, wy = waypoint - for neighbor in neighbors: - nx, ny = neighbor - ax2.plot([wx, nx], [wy, ny], 'g-', linewidth=1, alpha=0.6) - - # Plot waypoints as nodes - waypoint_coords = np.array(list(waypoint_graph.keys())) - ax2.plot(waypoint_coords[:, 0], waypoint_coords[:, 1], 'go', - markersize=8, label="Waypoints", zorder=3) - - ax2.set_xlabel('X position') - ax2.set_ylabel('Y position') - ax2.set_title('Waypoint Graph Connections') - ax2.legend() - ax2.grid(True, alpha=0.3) +def visualize(test_cases): + # Visualize one or more tracks with triangulation and waypoint graph + # test_cases: list of (name, cone_data) tuples + + n = len(test_cases) + cols = min(n, 3) + rows = (n + cols - 1) // cols + fig, axes = plt.subplots(rows, cols, figsize=(6*cols, 5*rows), squeeze=False) + axes = axes.flatten() + + for i, (name, cone_data) in enumerate(test_cases): + ax = axes[i] + points = np.array(cone_data) + waypoints, waypoint_graph, tri = get_midpoints(cone_data) + + # Plot triangulation and cones + ax.triplot(points[:, 0], points[:, 1], tri.simplices, 'b-', linewidth=0.5) + ax.plot(points[:, 0], points[:, 1], 'ro', markersize=6, label="Cones") + ax.plot(waypoints[:, 0], waypoints[:, 1], 'go', markersize=4, label="Waypoints") + + # Draw waypoint graph connections + for wp, neighbors in waypoint_graph.items(): + for neighbor in neighbors: + ax.plot([wp[0], neighbor[0]], [wp[1], neighbor[1]], 'g-', linewidth=0.8, alpha=0.4) + + ax.set_title(f'{name} ({len(points)} cones)') + ax.legend(fontsize=8) + ax.grid(True, alpha=0.3) + ax.set_aspect('equal', adjustable='box') + + # Hide unused subplots + for j in range(n, len(axes)): + axes[j].set_visible(False) plt.tight_layout() plt.show() @@ -155,10 +115,10 @@ def visualize_combined(points, tri, waypoints, waypoint_graph): # Choose which test data to use test_cases = [ + ("Random 30", random_scatter), ("Simple Track", simple_track), ("Slalom", slalom), ("Grid", grid), - ("Random 30", random_scatter), ("Oval Track", oval_track) ] @@ -171,5 +131,5 @@ def visualize_combined(points, tri, waypoints, waypoint_graph): print(f"{name:15} | Cones: {len(points):3} | Time: {elapsed*1000:6.2f} ms | Hz: {1/elapsed:6.1f}") - # Visualize the last test case - both plots side-by-side - visualize_combined(points, tri, waypoints, waypoint_graph) + # Visualize all test tracks + visualize(test_cases) From 3dc139861dbed1e6a7ce3cddaaa931fa1be71ac1 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Mon, 24 Nov 2025 19:14:58 -0800 Subject: [PATCH 13/18] updated env --- environment.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/environment.yml b/environment.yml index f9178ff..f048d15 100644 --- a/environment.yml +++ b/environment.yml @@ -116,7 +116,7 @@ dependencies: - pygame=2.6.1 - pyparsing=3.1.2 - pysocks=1.7.1 - - python=3.12.2 + - python=3.10.9 - python-dateutil=2.9.0post0 - python-tzdata=2023.3 - python_abi=3.12 @@ -236,4 +236,4 @@ dependencies: - watchfiles==1.1.0 - websockets==13.1 - zstd==1.5.7.2 -prefix: /Users/daniel/anaconda3/envs/fsae +prefix: /Users/username/anaconda3/envs/fsae From 9b40cf8754ff3b7d39c0706a2ec08b58db9ec7fe Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Wed, 26 Nov 2025 18:00:19 -0800 Subject: [PATCH 14/18] testing branch --- PathPlanning/path_planner.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/PathPlanning/path_planner.py b/PathPlanning/path_planner.py index a4828b8..d694eeb 100644 --- a/PathPlanning/path_planner.py +++ b/PathPlanning/path_planner.py @@ -11,3 +11,6 @@ from beam_search import BeamSearchPathPlanner from path_smoother import PathSmoother import config + +def test() : + return \ No newline at end of file From 82e19e57f8c5de90660d30ce21c1768e8c701851 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Wed, 26 Nov 2025 18:04:58 -0800 Subject: [PATCH 15/18] testing push --- PathPlanning/path_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/path_planner.py b/PathPlanning/path_planner.py index d694eeb..82643a0 100644 --- a/PathPlanning/path_planner.py +++ b/PathPlanning/path_planner.py @@ -13,4 +13,4 @@ import config def test() : - return \ No newline at end of file + return 1 \ No newline at end of file From 592475d05a87a518c16432165bacef9880cbbf81 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Mon, 1 Dec 2025 02:23:04 -0800 Subject: [PATCH 16/18] update path_tree.py --- PathPlanning/path_tree.py | 169 +++++++++++++++++++++++++++++++++++++- 1 file changed, 167 insertions(+), 2 deletions(-) diff --git a/PathPlanning/path_tree.py b/PathPlanning/path_tree.py index 524ff6d..26fd24a 100644 --- a/PathPlanning/path_tree.py +++ b/PathPlanning/path_tree.py @@ -4,6 +4,171 @@ """ import numpy as np -from typing import List, Optional -from dataclasses import dataclass +import matplotlib.pyplot as plt +import random import config +from delaunay import get_midpoints + +# Need vehicle_heading in RADIANS from localization +# Find k nearest waypoints in the forward arc (±90°) of the vehicle +def find_nearest_waypoints(vehicle_pos, vehicle_heading, waypoints, k): + # compute vector from vehicle position to each waypoint + vectors = waypoints - vehicle_pos + + # compute angles of each waypoint using arctan2 for 4 quadrant calc + angles = np.arctan2(vectors[:, 1], vectors[:, 0]) + angle_diff = angles - vehicle_heading + + # normalize angles to the range [-pi, pi] + angle_diff = np.arctan2(np.sin(angle_diff), np.cos(angle_diff)) + + # create boolean array to keep all waypoints that are in front of the car and apply filter + forward_mask = np.abs(angle_diff) < (np.pi / 2) + masked_waypoints = waypoints[forward_mask] + + # compute the magnitude of each vector using np.linalg.norm and get k shortest distances + distances = np.linalg.norm(masked_waypoints - vehicle_pos, axis=1) + k_indices = np.argsort(distances)[:k] + + # return the k nearest waypoints + k_waypoints = masked_waypoints[k_indices] + return [tuple(np.round(wp, decimals=6)) for wp in k_waypoints] + +# Generate path tree from cone positions (main API function) +def get_path_tree(cones, vehicle_pos, vehicle_heading, max_depth, k_start): + # get the possible waypoints + waypoints, waypoint_graph, tri = get_midpoints(cones) + + # get the k nearest waypoints + starting_waypoints = find_nearest_waypoints(vehicle_pos, vehicle_heading, waypoints, k_start) + + # Helper function to check if waypoint is forward of vehicle + def is_forward(waypoint): + # Check if waypoint is in forward half-plane from vehicle + vec = np.array(waypoint) - np.array(vehicle_pos) + # Gets vector in the direction of the vehicle + forward_dir = np.array([np.cos(vehicle_heading), np.sin(vehicle_heading)]) + # If the two vectors are in similar direction, their dot product will be > 0 + return np.dot(vec, forward_dir) > 0 + + current_level = [[wp] for wp in starting_waypoints] + for _ in range(max_depth): + next_level = [] + for path in current_level: + last = path[-1] + for next_wp in waypoint_graph.get(last, set()): + # Only add waypoints that are forward AND not already visited + if next_wp not in path and is_forward(next_wp): + next_level.append(path + [next_wp]) + + current_level = next_level + + return current_level + +# Visualize the paths with cones and vehicle position +def visualize_paths(paths, cones, vehicle_pos, vehicle_heading, max_paths=None): + + plt.figure(figsize=(14, 10)) + + # Convert to numpy array for easier plotting + cones = np.array(cones) + vehicle_pos = np.array(vehicle_pos) + + # Plot cones + plt.scatter(cones[:, 0], cones[:, 1], c='red', s=150, + marker='o', edgecolors='darkred', linewidth=2, + label='Cones', zorder=5, alpha=0.8) + + # Plot vehicle position + plt.scatter(vehicle_pos[0], vehicle_pos[1], c='blue', s=500, + marker='*', edgecolors='black', linewidth=2, + label='Vehicle', zorder=10) + + # Plot vehicle heading arrow + arrow_length = 0.8 + dx = arrow_length * np.cos(vehicle_heading) + dy = arrow_length * np.sin(vehicle_heading) + plt.arrow(vehicle_pos[0], vehicle_pos[1], dx, dy, + head_width=0.25, head_length=0.2, fc='blue', ec='black', + linewidth=1.5, zorder=10) + + # Randomly sample paths to display if specified + if max_paths and len(paths) > max_paths: + paths_to_show = random.sample(paths, max_paths) + else: + paths_to_show = paths + + # Plot all paths with different colors and styles + colors = ['green', 'orange', 'purple', 'cyan', 'magenta', + 'lime', 'pink', 'gold', 'brown', 'navy'] + line_styles = ['-', '--', '-.', ':'] # Solid, dashed, dash-dot, dotted + + for i, path in enumerate(paths_to_show): + if len(path) == 0: + continue + + # Convert path to numpy array for plotting + path_array = np.array(path) + color = colors[i % len(colors)] + linestyle = line_styles[i % len(line_styles)] + + # Plot path as connected line with style variation + plt.plot(path_array[:, 0], path_array[:, 1], + color=color, linewidth=2.5, alpha=0.8, linestyle=linestyle, + marker='o', markersize=7, markeredgecolor='black', + markeredgewidth=0.8, zorder=3) + + # Add path number at the end of each path + end_point = path_array[-1] + plt.text(end_point[0], end_point[1], f' {i+1}', + fontsize=10, fontweight='bold', color=color, + bbox=dict(boxstyle='round,pad=0.3', facecolor='white', + edgecolor=color, alpha=0.8), zorder=15) + + # Highlight starting waypoint with larger marker + start_point = path_array[0] + plt.scatter(start_point[0], start_point[1], + c=color, s=200, marker='s', edgecolors='black', + linewidth=2, zorder=8, alpha=0.9) + + # Add path count to legend + if len(paths) > 0: + if max_paths and len(paths) > max_paths: + label_text = f'{len(paths_to_show)}/{len(paths)} paths shown' + else: + label_text = f'{len(paths)} paths generated' + plt.plot([], [], color='gray', linewidth=2.5, label=label_text) + + plt.xlabel('X position (m)', fontsize=13, fontweight='bold') + plt.ylabel('Y position (m)', fontsize=13, fontweight='bold') + plt.title('Path Planning Visualization', fontsize=16, fontweight='bold', pad=20) + plt.legend(loc='best', fontsize=11, framealpha=0.9) + plt.grid(True, alpha=0.4, linestyle='--') + plt.axis('equal') + plt.tight_layout() + plt.show() + + +if __name__ == "__main__": + + oval_track = [ + # Outer boundary + [0, 0], [2, -1], [4, -1.5], [6, -1.5], [8, -1], [10, 0], + [10, 2], [8, 3], [6, 3.5], [4, 3.5], [2, 3], [0, 2], + # Inner boundary + [2, 0.5], [4, 0], [6, 0], [8, 0.5], [8, 1.5], [6, 2], + [4, 2], [2, 1.5] + ] + + vehicle_pos = [3, -0.5] # On racing line between inner/outer cones + vehicle_heading = np.radians(0) # Heading straight down the track + + paths = get_path_tree( + cones=oval_track, + vehicle_pos=vehicle_pos, + vehicle_heading=vehicle_heading, + max_depth=4, + k_start=3 + ) + + visualize_paths(paths, oval_track, vehicle_pos, vehicle_heading, max_paths=1) From e6aaa02c651e619e30492a0afad165fa26574001 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Sat, 13 Dec 2025 23:52:02 -0800 Subject: [PATCH 17/18] added path_smoother and minor updates --- PathPlanning/path_planner.py | 16 ++-- PathPlanning/path_smoother.py | 150 ++++++++++++++++++++++++++++++++-- PathPlanning/path_tree.py | 20 ++++- 3 files changed, 166 insertions(+), 20 deletions(-) diff --git a/PathPlanning/path_planner.py b/PathPlanning/path_planner.py index 82643a0..07c1e09 100644 --- a/PathPlanning/path_planner.py +++ b/PathPlanning/path_planner.py @@ -1,15 +1,9 @@ -""" -Main Path Planning Module -Deadline: Dec 6, 2025 -""" - import numpy as np -from typing import Tuple -from delaunay import create_delaunay_triangulation, get_midpoint_graph -from path_tree import PathTree -from cost_functions import PathCostEvaluator -from beam_search import BeamSearchPathPlanner -from path_smoother import PathSmoother +from delaunay import get_midpoints +from path_tree import get_path_tree +from cost_functions import +from beam_search import +from path_smoother import smooth_path import config def test() : diff --git a/PathPlanning/path_smoother.py b/PathPlanning/path_smoother.py index f98ed55..679cfcb 100644 --- a/PathPlanning/path_smoother.py +++ b/PathPlanning/path_smoother.py @@ -1,9 +1,145 @@ -""" -Path Smoothing using Spline Interpolation -Deadline: Dec 3, 2025 -""" - import numpy as np -from typing import List, Tuple -from scipy.interpolate import splprep, splev, CubicSpline +from scipy.interpolate import splprep, splev +import matplotlib.pyplot as plt import config + +def smooth_path(waypoints, num_points=100): + # u: parameter values of original waypoints + # that measure how far along the path each waypoint is + # given as a normalized value [0,1] + # tck: tuple(knots, coefficients, degree of spline) + tck, u = splprep([waypoints[:, 0], waypoints[:, 1]], s=0, k=3) + + # get num_points evenly spaced points + u_fine = np.linspace(0, 1, num_points) + + # get smoothed x,y coordinates and path + x_smooth, y_smooth = splev(u_fine, tck) + smooth_path = np.column_stack([x_smooth, y_smooth]) + + # get curvature (curvature = 1/radius) + dx_dt, dy_dt = splev(u_fine, tck, der=1) + d2x_dt2, d2y_dt2 = splev(u_fine, tck, der=2) + + curvature = (dx_dt * d2y_dt2 - dy_dt * d2x_dt2) / (dx_dt**2 + dy_dt**2)**1.5 + + return smooth_path, curvature + +def visualize(test_cases): + """ + Visualize smooth paths with curvature analysis. + + Args: + test_cases: List of (name, waypoints) tuples + """ + n = len(test_cases) + fig, axes = plt.subplots(n, 3, figsize=(18, 5*n), squeeze=False) + + for i, (name, waypoints) in enumerate(test_cases): + # Smooth the path + smooth, curvature = smooth_path(waypoints, num_points=200) + + # Calculate turn radius (clipped for visualization) + radius = np.zeros_like(curvature) + for j, k in enumerate(curvature): + radius[j] = 1.0 / abs(k) if abs(k) > 0.001 else 100.0 + radius = np.clip(radius, 0, 50) + + # Calculate path progress percentage + path_progress = np.linspace(0, 100, len(curvature)) + + # Plot 1: Path with curvature coloring + ax1 = axes[i, 0] + scatter = ax1.scatter(smooth[:, 0], smooth[:, 1], + c=np.abs(curvature), cmap='RdYlGn_r', + s=20, vmin=0, vmax=0.3) + ax1.plot(waypoints[:, 0], waypoints[:, 1], 'ko-', + markersize=8, linewidth=1, alpha=0.5, label='Input waypoints') + ax1.set_title(f'{name} - Smooth Path (colored by curvature)', fontweight='bold') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.axis('equal') + ax1.grid(True, alpha=0.3) + ax1.legend() + cbar = plt.colorbar(scatter, ax=ax1) + cbar.set_label('|Curvature| (1/m)') + + # Plot 2: Curvature profile + ax2 = axes[i, 1] + ax2.plot(path_progress, curvature, 'b-', linewidth=2) + ax2.axhline(y=0, color='k', linestyle='-', linewidth=0.5) + ax2.axhline(y=0.222, color='r', linestyle='--', linewidth=1.5, + label='Max allowed (9m diameter)') + ax2.axhline(y=-0.222, color='r', linestyle='--', linewidth=1.5) + ax2.fill_between(path_progress, -0.222, 0.222, alpha=0.2, color='green') + ax2.set_title('Curvature Profile', fontweight='bold') + ax2.set_xlabel('Path Progress (%)') + ax2.set_ylabel('Curvature (1/m)') + ax2.grid(True, alpha=0.3) + ax2.legend() + + # Plot 3: Turn radius + ax3 = axes[i, 2] + ax3.plot(path_progress, radius, 'g-', linewidth=2) + ax3.fill_between(path_progress, radius, 50, alpha=0.3, color='green') + ax3.axhline(y=4.5, color='r', linestyle='--', linewidth=1.5, + label='Min allowed (4.5m)') + ax3.fill_between(path_progress, 0, 4.5, alpha=0.2, color='red') + ax3.set_title('Turn Radius', fontweight='bold') + ax3.set_xlabel('Path Progress (%)') + ax3.set_ylabel('Radius (m)') + ax3.set_ylim([0, 50]) + ax3.grid(True, alpha=0.3) + ax3.legend() + + # Print statistics + max_curv = np.max(np.abs(curvature)) + min_radius = 1.0 / max_curv if max_curv > 0 else float('inf') + status = "✓ PASS" if max_curv <= 0.222 else "✗ FAIL" + print(f"{name:20s} | Max curvature: {max_curv:.4f} | Min radius: {min_radius:.2f}m | {status}") + + plt.tight_layout() + plt.show() + +if __name__ == "__main__": + # Test track configurations + straight_line = np.array([ + [0, 0], [5, 0], [10, 0], [15, 0], [20, 0] + ]) + + simple_curve = np.array([ + [0, 0], [5, 0], [10, 2], [15, 5], [20, 5] + ]) + + hairpin = np.array([ + [0, 0], [10, 0], [15, 5], [15, 10], + [10, 15], [5, 15], [0, 10], [0, 5] + ]) + + slalom = np.array([ + [0, 0], [5, 2], [10, 0], [15, -2], + [20, 0], [25, 2], [30, 0] + ]) + + oval_track = np.array([ + [0, 0], [10, 0], [20, 0], [25, 5], [25, 10], + [20, 15], [10, 15], [0, 15], [-5, 10], [-5, 5] + ]) + + s_curve = np.array([ + [0, 0], [5, 2], [10, 5], [15, 7], + [20, 7], [25, 5], [30, 2], [35, 0] + ]) + + # Test cases + test_cases = [ + ("Straight Line", straight_line), + ("Simple Curve", simple_curve), + ("Hairpin Turn", hairpin), + ("Slalom", slalom), + ("Oval Track", oval_track), + ("S-Curve", s_curve) + ] + + # Visualize + visualize(test_cases) diff --git a/PathPlanning/path_tree.py b/PathPlanning/path_tree.py index 26fd24a..1aa1f9e 100644 --- a/PathPlanning/path_tree.py +++ b/PathPlanning/path_tree.py @@ -74,6 +74,22 @@ def visualize_paths(paths, cones, vehicle_pos, vehicle_heading, max_paths=None): cones = np.array(cones) vehicle_pos = np.array(vehicle_pos) + # Get all possible waypoints from Delaunay triangulation + waypoints, _, _ = get_midpoints(cones) + + # Filter to only forward-facing waypoints (±90° arc) + vectors = waypoints - vehicle_pos + angles = np.arctan2(vectors[:, 1], vectors[:, 0]) + angle_diff = angles - vehicle_heading + angle_diff = np.arctan2(np.sin(angle_diff), np.cos(angle_diff)) + forward_mask = np.abs(angle_diff) < (np.pi / 2) + masked_waypoints = waypoints[forward_mask] + + # Plot only masked waypoints (small but visible) + plt.scatter(masked_waypoints[:, 0], masked_waypoints[:, 1], c='lightgray', s=30, + marker='o', edgecolors='gray', linewidth=0.5, + label='Available Waypoints', zorder=2, alpha=0.7) + # Plot cones plt.scatter(cones[:, 0], cones[:, 1], c='red', s=150, marker='o', edgecolors='darkred', linewidth=2, @@ -168,7 +184,7 @@ def visualize_paths(paths, cones, vehicle_pos, vehicle_heading, max_paths=None): vehicle_pos=vehicle_pos, vehicle_heading=vehicle_heading, max_depth=4, - k_start=3 + k_start=2 ) - visualize_paths(paths, oval_track, vehicle_pos, vehicle_heading, max_paths=1) + visualize_paths(paths, oval_track, vehicle_pos, vehicle_heading, max_paths=10) From aa48a3dffe8a170f755b98673dd35657938dd292 Mon Sep 17 00:00:00 2001 From: nryee2005 Date: Sat, 13 Dec 2025 23:56:37 -0800 Subject: [PATCH 18/18] update README --- PathPlanning/README.md | 126 +++++++++++++++++++++++++---------------- 1 file changed, 78 insertions(+), 48 deletions(-) diff --git a/PathPlanning/README.md b/PathPlanning/README.md index 8d924c7..8c97bf2 100644 --- a/PathPlanning/README.md +++ b/PathPlanning/README.md @@ -9,6 +9,22 @@ This module implements real-time path planning for the UCSC FSAE driverless vehi - **Reference**: arXiv:1905.05150v1, Section 4.3 - **GitHub**: https://github.com/AMZ-Driverless +## Current Status (Dec 2025) + +**Overall Progress**: 3 of 5 core phases complete (60%) + +✅ **Completed**: +- Delaunay Triangulation with vectorized performance (0.8-2.5ms, 400-1250 Hz) +- Path Tree Population with breadth-first search and forward-arc filtering +- Path Smoothing with cubic splines and Formula Student constraint validation + +⏳ **Remaining**: +- Cost Function implementation (5 AMZ terms) +- Beam Search pruning for path selection +- Control Planning interface definition + +**Performance**: Currently well within 40ms budget. Delaunay + smoothing = ~5-12ms total. + ## Module Overview ### Core Files @@ -20,23 +36,27 @@ This module implements real-time path planning for the UCSC FSAE driverless vehi - Vehicle dynamics parameters - Sensor range (~10m) -2. **delaunay.py** - Delaunay Triangulation **[IN PROGRESS]** +2. **delaunay.py** - Delaunay Triangulation ✅ **[COMPLETE]** - Uses `scipy.spatial.Delaunay` for triangulation (NOT CGAL) - Creates triangulation from cone [x, y] positions - - Extracts midpoint graph for waypoints + - Extracts midpoint graph for waypoints with adjacency connections - **Important**: Handles duplicate midpoints (interior edges shared by 2 triangles) - - Visualization utilities (matplotlib) + - Vectorized implementation for performance (>100 Hz on test data) + - Multi-track visualization utilities (matplotlib) - **AMZ Implementation**: Used CGAL library, we use scipy for simplicity - - **Status**: In Progress (Nov 8-15) + - **Performance**: 0.8-2.5ms for 10-30 cones (400-1250 Hz) + - **Status**: ✅ Complete -3. **path_tree.py** - Path Tree Population (Breadth-First) +3. **path_tree.py** - Path Tree Population (Breadth-First) ✅ **[COMPLETE]** - Builds breadth-first tree of possible paths through midpoint graph - - Starting node: current vehicle position - - Manages PathNode objects with parent/child relationships - - Each path connects to center points of surrounding edges + - `find_nearest_waypoints()`: Selects k nearest waypoints in forward arc (±90°) + - `get_path_tree()`: Grows tree from vehicle position with configurable depth + - Prevents loops by tracking visited waypoints in each path + - Forward-only constraint (no backwards waypoints) - Limited tree depth to meet 40ms cycle time constraint - - **AMZ approach**: Breadth-first manor, fixed iteration limit - - **Status**: Not Started (Deadline: Nov 22) + - Rich visualization showing paths, cones, vehicle, and heading + - **AMZ approach**: Breadth-first manner, fixed iteration limit + - **Status**: ✅ Complete 4. **cost_functions.py** - Path Cost Evaluation (5 AMZ Terms) @@ -79,14 +99,15 @@ This module implements real-time path planning for the UCSC FSAE driverless vehi - **AMZ result**: Only 4.2% of paths left track, mostly at >7m distance - **Status**: Not Started (Deadline: Nov 29) -6. **path_smoother.py** - Spline Interpolation - - Smooths discrete waypoints using scipy.interpolate - - Options: splprep/splev (parametric splines) or CubicSpline - - Calculates curvature profiles for velocity planning - - Generates velocity profiles respecting dynamics - - Resampling utilities for control planning +6. **path_smoother.py** - Spline Interpolation ✅ **[COMPLETE]** + - `smooth_path()`: Smooths discrete waypoints using cubic splines (splprep/splev) + - Calculates curvature at each point: κ = (dx·d²y - dy·d²x) / (dx² + dy²)^1.5 + - Validates against Formula Student constraints (min 9m turning diameter) + - Multi-panel visualization: path with curvature coloring, curvature profile, turn radius + - Pass/fail indicators for each test track + - Test suite includes 6 track configurations (straight, curve, hairpin, slalom, oval, s-curve) - **AMZ approach**: Feeds smoothed path to pure pursuit controller - - **Status**: Not Started (Deadline: Dec 3) + - **Status**: ✅ Complete 7. **path_planner.py** - Main Integration Module - 25 Hz planning loop (40ms cycle time) @@ -162,37 +183,45 @@ SLAM -> Cones -> Delaunay -> Waypoints -> Path Tree -> Beam Search -> Best Path Each module has standalone tests in its `if __name__ == "__main__"` block: ```bash -# Test Delaunay triangulation (CURRENTLY WORKING) +# Test Delaunay triangulation ✅ python PathPlanning/delaunay.py -# Test path tree (after implementation) +# Test path tree ✅ python PathPlanning/path_tree.py -# Test cost functions (after implementation) +# Test path smoother ✅ +python PathPlanning/path_smoother.py + +# Test cost functions (not yet implemented) python PathPlanning/cost_functions.py -# Test beam search (after implementation) +# Test beam search (not yet implemented) python PathPlanning/beam_search.py -# Test path smoother (after implementation) -python PathPlanning/path_smoother.py - -# Test full integrated system (after implementation) +# Test full integrated system (not yet implemented) python PathPlanning/path_planner.py ``` -### Delaunay Test Patterns +### Test Outputs -The delaunay.py script includes multiple test patterns (modify `test_cone_data` variable): +**delaunay.py**: +- Prints performance benchmarks for 5 test tracks (cones, time, Hz) +- Shows multi-panel visualization of triangulation + waypoint graph +- Validates vectorized implementation performance -```python -# Available test patterns: -test_cone_data = oval_track # Realistic oval racing circuit -test_cone_data = simple_track # Basic straight-to-turn layout -test_cone_data = slalom # Chicane/slalom pattern -test_cone_data = grid # Stress test with regular grid -test_cone_data = random_scatter # Random cones for robustness test -``` +**path_tree.py**: +- Generates all possible paths from vehicle position through waypoint graph +- Visualizes up to 10 sample paths with different colors/styles +- Shows vehicle position, heading arrow, cones, and available waypoints +- Forward-arc filtering demonstration (±90° from vehicle heading) + +**path_smoother.py**: +- Generates 6 test tracks (straight, simple curve, hairpin, slalom, oval, s-curve) +- Shows 3-panel visualization for each track: + - Path with curvature color coding (red = high curvature) + - Curvature profile vs. path progress + - Turn radius with Formula Student compliance zones +- Prints pass/fail status for min 9m turning diameter constraint ## Performance Requirements @@ -202,14 +231,15 @@ test_cone_data = random_scatter # Random cones for robustness test - **Robustness**: Handle noisy monocular vision data - **AMZ benchmark**: 50ms cycle time in SLAM mode, achieved 1.5g lateral acceleration -### Computational Budget +### Computational Budget (Current Performance) -- Delaunay triangulation: ~5-10ms (scipy is fast) -- Tree building: Variable (limit iterations) -- Cost evaluation: ~5-10ms (vectorize with NumPy) -- Beam search: ~5ms (fixed beam width) -- Spline smoothing: ~5ms +- Delaunay triangulation: ✅ 0.8-2.5ms for 10-30 cones (400-1250 Hz) +- Tree building: TBD (depends on max_depth and k_start parameters) +- Cost evaluation: ⏳ Not yet implemented +- Beam search: ⏳ Not yet implemented +- Spline smoothing: ✅ ~5-10ms (acceptable for 25 Hz) - **Total target**: <40ms on Raspberry Pi +- **Current progress**: 3/5 phases complete, well within budget so far ## Key Implementation Differences from AMZ @@ -247,13 +277,13 @@ test_cone_data = random_scatter # Random cones for robustness test | Task | Deadline | Status | |------|----------|--------| -| Delaunay Triangulation (scipy) | Nov 8 | **IN PROGRESS** | -| Test & Visualize | Nov 15 | **IN PROGRESS** | -| Path Tree Population | Nov 22 | Not Started | -| Beam Search Pruning | Nov 29 | Not Started | -| Cost Functions (5 AMZ terms) | Dec 3 | Not Started | -| Path Smoothing (scipy splines) | Dec 3 | Not Started | -| Control Interface | Dec 6 | Not Started | +| Delaunay Triangulation (scipy) | Nov 8 | ✅ **COMPLETE** | +| Test & Visualize | Nov 15 | ✅ **COMPLETE** | +| Path Tree Population | Nov 22 | ✅ **COMPLETE** | +| Path Smoothing (scipy splines) | Dec 3 | ✅ **COMPLETE** | +| Beam Search Pruning | Nov 29 | ⏳ **NOT STARTED** | +| Cost Functions (5 AMZ terms) | Dec 3 | ⏳ **NOT STARTED** | +| Control Interface | Dec 6 | ⏳ **NOT STARTED** | ## Dependencies