Skip to content

Commit 06d3f00

Browse files
Lana243Lana243
committed
seminar06-planning
1 parent a81c0b3 commit 06d3f00

82 files changed

Lines changed: 77937 additions & 0 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎seminar06-planning/planner.ipynb‎

Lines changed: 388 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,388 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "markdown",
5+
"metadata": {},
6+
"source": [
7+
"#### Install python dependencies"
8+
]
9+
},
10+
{
11+
"cell_type": "code",
12+
"execution_count": null,
13+
"metadata": {},
14+
"outputs": [],
15+
"source": [
16+
"%pip install jsonpickle dacite shapely"
17+
]
18+
},
19+
{
20+
"cell_type": "markdown",
21+
"metadata": {},
22+
"source": [
23+
"#### Run the planning server (should be executed only once)"
24+
]
25+
},
26+
{
27+
"cell_type": "code",
28+
"execution_count": null,
29+
"metadata": {},
30+
"outputs": [],
31+
"source": [
32+
"import py_planning\n",
33+
"py_planning.init()"
34+
]
35+
},
36+
{
37+
"cell_type": "markdown",
38+
"metadata": {},
39+
"source": [
40+
"#### Visualization"
41+
]
42+
},
43+
{
44+
"cell_type": "code",
45+
"execution_count": null,
46+
"metadata": {},
47+
"outputs": [],
48+
"source": [
49+
"# # you can also open http://127.0.0.1:8008 in your browser\n",
50+
"\n",
51+
"from IPython.display import IFrame\n",
52+
"IFrame('http://127.0.0.1:8008', width=\"100%\", height=650)"
53+
]
54+
},
55+
{
56+
"cell_type": "markdown",
57+
"metadata": {},
58+
"source": [
59+
"#### Lane centering"
60+
]
61+
},
62+
{
63+
"cell_type": "code",
64+
"execution_count": null,
65+
"metadata": {},
66+
"outputs": [],
67+
"source": [
68+
"from enum import IntEnum\n",
69+
"import math\n",
70+
"import numpy as np"
71+
]
72+
},
73+
{
74+
"cell_type": "code",
75+
"execution_count": null,
76+
"metadata": {
77+
"scrolled": true
78+
},
79+
"outputs": [],
80+
"source": [
81+
"from py_planning.data_types import PlannedPath, PlannedState, State, Position # data types used by planner interface\n",
82+
"from shapely.geometry import LineString, Point\n",
83+
"\n",
84+
"import time\n",
85+
"\n",
86+
"\"\"\"\n",
87+
"find closest point on a polyline to the given point\n",
88+
"\"\"\"\n",
89+
"def get_index_of_closest_point(line: LineString, point: Point):\n",
90+
" closest_point_index = None\n",
91+
" min_distance = float('inf')\n",
92+
"\n",
93+
" for i, line_point in enumerate(line.coords):\n",
94+
" line_point = Point(line_point)\n",
95+
" distance = point.distance(line_point)\n",
96+
" if distance < min_distance:\n",
97+
" min_distance = distance\n",
98+
" closest_point_index = i\n",
99+
"\n",
100+
" return closest_point_index\n",
101+
"\n",
102+
"\n",
103+
"\"\"\"\n",
104+
"This function is called by the simulator for each tick.\n",
105+
"It should return recent planned trajectory up to date with the environment state.\n",
106+
"'state' parameter contains current world observations and vehicle state.\n",
107+
"\"\"\"\n",
108+
"def do_plan(state: State) -> PlannedPath:\n",
109+
" vehicle_pose = state.vehicle_pose\n",
110+
" vehicle_pos = Point(vehicle_pose.pos.x, vehicle_pose.pos.y) # current position of the AV\n",
111+
"\n",
112+
" centerline = LineString([(p.x, p.y) for p in state.lane_path.centerline])\n",
113+
"\n",
114+
" closest_index = get_index_of_closest_point(centerline, vehicle_pos)\n",
115+
" current_velocity = vehicle_pose.velocity\n",
116+
"\n",
117+
" # we leave some previous poses to make AV control stable\n",
118+
" prev_poses_count = 3\n",
119+
" max_poses_count = 50\n",
120+
" first_pose_index = max(closest_index - prev_poses_count, 0)\n",
121+
"\n",
122+
" # as a baseline here we just follow the centerline\n",
123+
" planned_states = [\n",
124+
" PlannedState(pos=p, velocity=current_velocity) for p in state.lane_path.centerline\n",
125+
" ][first_pose_index:first_pose_index+ max_poses_count]\n",
126+
"\n",
127+
" return PlannedPath(states=planned_states)\n",
128+
" \n",
129+
"\n",
130+
"# run the case in the simulator, watch the visualization\n",
131+
"py_planning.run_planner(\n",
132+
" do_plan,\n",
133+
" stop_on_fail=True # set to False to continue planning after case fail (useful for debugging)\n",
134+
")"
135+
]
136+
},
137+
{
138+
"cell_type": "markdown",
139+
"metadata": {},
140+
"source": [
141+
"#### Graph geometry planning"
142+
]
143+
},
144+
{
145+
"cell_type": "code",
146+
"execution_count": null,
147+
"metadata": {},
148+
"outputs": [],
149+
"source": [
150+
"def create_rotation_matrix(yaw):\n",
151+
" T = np.zeros((len(yaw), 2, 2))\n",
152+
" T[:, 0, 0] = np.cos(yaw)\n",
153+
" T[:, 0, 1] = -np.sin(yaw)\n",
154+
" T[:, 1, 0] = np.sin(yaw)\n",
155+
" T[:, 1, 1] = np.cos(yaw)\n",
156+
"\n",
157+
" return T\n",
158+
" \n",
159+
"class Layer():\n",
160+
" class Id(IntEnum):\n",
161+
" X = 0\n",
162+
" Y = 1\n",
163+
" YAW = 2\n",
164+
" COST = 3\n",
165+
" PARENT = 4\n",
166+
" SIZE = 5\n",
167+
"\n",
168+
" def __init__(self, N=None, nodes=None):\n",
169+
" assert (N is None) ^ (nodes is None)\n",
170+
" if N is not None:\n",
171+
" self.nodes = np.zeros((N, Layer.Id.SIZE))\n",
172+
" if nodes is not None:\n",
173+
" assert nodes.shape[1] == Layer.Id.SIZE\n",
174+
" self.nodes = nodes\n",
175+
" \n",
176+
" @property\n",
177+
" def x(self):\n",
178+
" return self.nodes[:, Layer.Id.X]\n",
179+
" \n",
180+
" @property\n",
181+
" def y(self):\n",
182+
" return self.nodes[:, Layer.Id.Y]\n",
183+
" \n",
184+
" @property\n",
185+
" def yaw(self):\n",
186+
" return self.nodes[:, Layer.Id.YAW]\n",
187+
" \n",
188+
" @property\n",
189+
" def cost(self):\n",
190+
" return self.nodes[:, Layer.Id.COST]\n",
191+
" \n",
192+
" @property\n",
193+
" def parent(self):\n",
194+
" return self.nodes[:, Layer.Id.PARENT]\n",
195+
" \n",
196+
" @property\n",
197+
" def N(self):\n",
198+
" return self.nodes.shape[0]\n",
199+
" \n",
200+
" @property\n",
201+
" def M(self):\n",
202+
" return self.nodes.shape[1]\n",
203+
" \n",
204+
" \n",
205+
"def arc_primitive(c, ds):\n",
206+
" if c == 0:\n",
207+
" return 0, ds, 0\n",
208+
" else:\n",
209+
" dyaw = c * ds\n",
210+
" return dyaw, 1 / c * math.sin(dyaw), 1 / c * (1 - math.cos(dyaw))\n",
211+
"\n",
212+
"\n",
213+
"class Graph(list):\n",
214+
" def nodes_num(self):\n",
215+
" nodes = 0\n",
216+
" for layer in self:\n",
217+
" nodes += layer.N\n",
218+
" return nodes\n",
219+
"\n",
220+
"\n",
221+
"def search(initial_state, lane_path, obstacles, curvature_primitives=[-0.2, 0., 0.2], ds=1, tree_depth=6, sparse=True):\n",
222+
" graph = Graph()\n",
223+
" initial_layer = Layer(1)\n",
224+
" initial_layer.nodes[:, Layer.Id.X] = initial_state.vehicle_pose.pos.x\n",
225+
" initial_layer.nodes[:, Layer.Id.Y] = initial_state.vehicle_pose.pos.y\n",
226+
" initial_layer.nodes[:, Layer.Id.YAW] = initial_state.vehicle_pose.rot\n",
227+
" graph.append(initial_layer) \n",
228+
" \n",
229+
" for i in range(tree_depth):\n",
230+
" X_c = graph[-1]\n",
231+
" X_n = _make_step(X_c, ds, curvature_primitives, lane_path, obstacles)\n",
232+
" if sparse:\n",
233+
" X_n = _sparsify(X_n)\n",
234+
"\n",
235+
" graph.append(X_n)\n",
236+
"\n",
237+
" return graph, _restore_path(graph, np.argmin(graph[-1].nodes[:, Layer.Id.COST]))\n",
238+
"\n",
239+
"\n",
240+
"def _make_step(X_c, ds, curvature_primitives, lane_path, obstacles):\n",
241+
" N = X_c.N\n",
242+
" X_n = Layer(N * len(curvature_primitives))\n",
243+
"\n",
244+
" for i, c in enumerate(curvature_primitives):\n",
245+
" # assumme instant change of curvature and movement along circle\n",
246+
" dyaw, dx, dy = arc_primitive(c, ds)\n",
247+
" shift = np.array([dx, dy])\n",
248+
"\n",
249+
" yaw_c = X_c.yaw\n",
250+
" T = create_rotation_matrix(yaw_c)\n",
251+
"\n",
252+
" X_n.x[i * N : (i + 1) * N] = X_c.x + T[:, 0] @ shift\n",
253+
" X_n.y[i * N : (i + 1) * N] = X_c.y + T[:, 1] @ shift\n",
254+
" X_n.yaw[i * N : (i + 1) * N] = yaw_c + dyaw\n",
255+
" X_n.parent[i * N : (i + 1) * N] = np.arange(N)\n",
256+
" X_n.cost[i * N : (i + 1) * N] = X_c.cost + c ** 2 \n",
257+
" # _update_cost(X_n.nodes[i * N : (i + 1) * N, :], lane_path, obstacles)\n",
258+
"\n",
259+
" return X_n\n",
260+
"\n",
261+
"\n",
262+
"# def _update_cost(X_n, lane_path, obstacles):\n",
263+
"# centerline = LineString([(p.x, p.y) for p in lane_path])\n",
264+
"# for i, node in enumerate(X_n):\n",
265+
"# _, d = get_index_of_closest_point(centerline, Point(node[Layer.Id.X], node[Layer.Id.Y]))\n",
266+
"# X_n[i, Layer.Id.COST] += d\n",
267+
"# # obstacles = get_closest_static_obstacles(obstacles, node[Layer.Id.X], node[Layer.Id.Y], 1)\n",
268+
"# # if len(obstacles) > 0:\n",
269+
"# # d_to_closest_static = dist(node[Layer.Id.X], node[Layer.Id.Y], obstacles[0])\n",
270+
"# # if d_to_closest_static < 2 * max(obstacles[0].w, obstacles[0].h):\n",
271+
"# # X_n[i, Layer.Id.COST] = np.inf\n",
272+
"# # else:\n",
273+
"# # X_n[i, Layer.Id.COST] += 10 * np.exp(-d_to_closest_static + 2 * max(obstacles[0].w, obstacles[0].h))\n",
274+
" \n",
275+
"\n",
276+
"\n",
277+
"def _sparsify(layer, min_nodes=5, step_x=1, step_y=1,step_yaw=0.1):\n",
278+
" if layer.N < min_nodes:\n",
279+
" return layer\n",
280+
"\n",
281+
" def node_to_key(x, y, yaw):\n",
282+
" return (round(x / step_x), round(y / step_y), round(yaw / step_yaw))\n",
283+
" d = {}\n",
284+
" for i in range(layer.N):\n",
285+
" key = node_to_key(layer.x[i], layer.y[i], layer.yaw[i])\n",
286+
" if key in d:\n",
287+
" d[key] = min(d[key], (layer.cost[i], i))\n",
288+
" else:\n",
289+
" d[key] = (layer.cost[i], i)\n",
290+
" indx = list(map(lambda value: value[1][1], d.items()))\n",
291+
" layer.nodes = layer.nodes[indx]\n",
292+
"\n",
293+
" return layer\n",
294+
"\n",
295+
"\n",
296+
"def _restore_path(graph, i):\n",
297+
" path = Graph()\n",
298+
" for j in range(len(graph)):\n",
299+
" layer = graph[-j - 1]\n",
300+
" path.append(Layer(nodes=np.copy(layer.nodes[i:i+1])))\n",
301+
" i = int(layer.parent[i])\n",
302+
"\n",
303+
" # fix parent linkage\n",
304+
" path[-1].parent[:] = 0\n",
305+
"\n",
306+
" path.reverse()\n",
307+
" return path"
308+
]
309+
},
310+
{
311+
"cell_type": "code",
312+
"execution_count": null,
313+
"metadata": {},
314+
"outputs": [],
315+
"source": [
316+
"from py_planning.data_types import PlannedPath, PlannedState, State, Position # data types used by planner interface\n",
317+
"from shapely.geometry import LineString, Point\n",
318+
"import matplotlib.pyplot as plt\n",
319+
"\n",
320+
"\"\"\"\n",
321+
"find closest point on a polyline to the given point\n",
322+
"\"\"\"\n",
323+
"def get_index_of_closest_point(line: LineString, point: Point):\n",
324+
" closest_point_index = None\n",
325+
" min_distance = float('inf')\n",
326+
"\n",
327+
" for i, line_point in enumerate(line.coords):\n",
328+
" line_point = Point(line_point)\n",
329+
" distance = point.distance(line_point)\n",
330+
" if distance < min_distance:\n",
331+
" min_distance = distance\n",
332+
" closest_point_index = i\n",
333+
"\n",
334+
" return closest_point_index, min_distance\n",
335+
"\n",
336+
"\n",
337+
"def dist(x, y, static_obstacle):\n",
338+
" return (x - static_obstacle.p[0]) ** 2 + (y - static_obstacle.p[1]) ** 2\n",
339+
"\n",
340+
"\n",
341+
"def get_closest_static_obstacles(static_obstacles,x, y, k):\n",
342+
" obstacles = sorted(static_obstacles, key=lambda obstacle: dist(x, y, obstacle))\n",
343+
" return obstacles[:min(len(obstacles), k)]\n",
344+
"\n",
345+
"\n",
346+
"def do_graph_planning(state: State) -> PlannedPath:\n",
347+
" vehicle_pose = state.vehicle_pose\n",
348+
" vehicle_pos = Point(vehicle_pose.pos.x, vehicle_pose.pos.y) # current position of the AV\n",
349+
" centerline = LineString([(p.x, p.y) for p in state.lane_path.centerline])\n",
350+
" closest_index, _ = get_index_of_closest_point(centerline, vehicle_pos)\n",
351+
" lane_path = state.lane_path.centerline[max(0, closest_index - 20) : min(len(state.lane_path.centerline), closest_index + 20) : 2]\n",
352+
" obstacles = get_closest_static_obstacles(state.static_obstacles, state.vehicle_pose.pos.x, state.vehicle_pose.pos.y, 1)\n",
353+
"\n",
354+
" ds = 1\n",
355+
" graph, path = search(state, lane_path, obstacles, tree_depth=12, ds=ds)\n",
356+
" planned_path = list(map(lambda layer: PlannedState(pos=Position(float(layer.nodes[0, Layer.Id.X]), float(layer.nodes[0, Layer.Id.Y])), velocity=state.vehicle_pose.velocity, rot=float(layer.nodes[0, Layer.Id.YAW])), path))\n",
357+
" return PlannedPath(states=planned_path) \n",
358+
"\n",
359+
"\n",
360+
"py_planning.run_planner(\n",
361+
" do_graph_planning,\n",
362+
" stop_on_fail=True # set to False to continue planning after case fail (useful for debugging)\n",
363+
")"
364+
]
365+
}
366+
],
367+
"metadata": {
368+
"kernelspec": {
369+
"display_name": "Python 3 (ipykernel)",
370+
"language": "python",
371+
"name": "python3"
372+
},
373+
"language_info": {
374+
"codemirror_mode": {
375+
"name": "ipython",
376+
"version": 3
377+
},
378+
"file_extension": ".py",
379+
"mimetype": "text/x-python",
380+
"name": "python",
381+
"nbconvert_exporter": "python",
382+
"pygments_lexer": "ipython3",
383+
"version": "3.11.7"
384+
}
385+
},
386+
"nbformat": 4,
387+
"nbformat_minor": 4
388+
}

‎seminar06-planning/py_planning/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
from .py_planning import init, run_planner
2+
from . import data_types

0 commit comments

Comments
 (0)