tutorial_route_planner.ipynb 8.54 KB
Newer Older
Edmond Irani Liu's avatar
Edmond Irani Liu committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial: Route Planner\n",
    "\n",
    "This tutorial demonstrates how the Route Planner package can be used to plan high-level routes for planning problems given in CommonRoad scenarios."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "* you have gone through the tutorial for **CommonRoad Input-Output**.\n",
    "* you have installed all necessary modules for **CommonRoad Search** according to the installation manual.\n",
    "\n",
    "Let's start with importing relevant modules and classes for setting up the automaton and the CommonRoad (CR) scenario."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "import os\n",
    "import sys\n",
    "\n",
    "path_notebook = os.getcwd()\n",
    "# add the route planner folder to python path\n",
    "sys.path.append(os.path.join(path_notebook, \"../../\"))\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad.visualization.draw_dispatch_cr import draw_object\n",
    "\n",
    "from SMP.route_planner.route_planner.route_planner import RoutePlanner\n",
    "from SMP.route_planner.route_planner.utils_visualization import draw_route, get_plot_limits_from_reference_path, \\\n",
    "    get_plot_limits_from_routes"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Loading CR Scenario and Planning Problem\n",
    "In the next step, we load a CommonRoad scenario and its planning problem(s), for which the routes should be planned. The route planner handles **one planning problem** at a time, thus we need to manually specify which planning problem it should take care. In our case, we select the first planning problem in the planning problem set. The meaning of the symbols in a scenario are explained as follows:\n",
    "* **Dot**: initial state of the planning problem projected onto the position domain\n",
    "* **Blue rectangle**: dynamic obstacle\n",
    "* **Yellow rectangle**: goal region projected onto the position domain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load scenario\n",
    "path_scenario = os.path.join(path_notebook, \"../../SMP/route_planner/scenarios/\")\n",
    "id_scenario = 'USA_Peach-2_1_T-1'\n",
    "\n",
    "# read in scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(path_scenario + id_scenario + '.xml').open()\n",
    "# retrieve the first planning problem in the problem set\n",
    "planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]\n",
    "    \n",
    "# plot the scenario and the planning problem set\n",
    "plt.figure(figsize=(12, 12))\n",
    "draw_object(scenario)\n",
    "draw_object(planning_problem_set)\n",
    "plt.gca().set_aspect('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Creating a route planner and planning for routes\n",
    "\n",
    "### 2.1 Instantiation\n",
    "A route planner can be easily constructed by passing the **scenario** and the **planning problem** to `RoutePlanner` object. As for the backend, there are currently three supported options:\n",
    "1. NETWORKX: uses built-in functions from the networkx package, tends to change lane later\n",
    "2. NETWORKX_REVERSED: uses built-in functions from the networkx package, tends to change lane earlier\n",
    "3. PRIORITY_QUEUE: uses A-star search to find routes, lane change maneuver depends on the heuristic cost\n",
    "\n",
    "### 2.2 Planning all possible routes\n",
    "The route planner plans a route for all possible combinations of start / goal lanelets. E.g. if our initial state is located in two lanes (due to overlapping of lanelets), and the same for our goal state, the route planner will try to plan routes for the four possible combinations.\n",
    "\n",
    "### 2.3 Retrieving a route\n",
    "Planned routes can be retrieved by using simple indices, or based on some heuristic functions to determine the best route of all. A route consists of a list of lanelet ids that leads from the initial state to the goal state.\n",
    "\n",
    "### 2.4 Retrieving reference path\n",
    "A reference path is automatically generated for each planned routes. The center lines of lanelets of a route is used to construct the reference path. The resulting polyline is then smoothened with Chaikin's corner cutting algorithm."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# instantiate a route planner\n",
    "route_planner = RoutePlanner(scenario, planning_problem, backend=RoutePlanner.Backend.NETWORKX_REVERSED)\n",
    "\n",
    "# plan routes, and save the found routes in a route candidate holder\n",
    "candidate_holder = route_planner.plan_routes()\n",
    "\n",
    "# retrieving routes\n",
    "# option 1: retrieve all routes\n",
    "list_routes, num_route_candidates = candidate_holder.retrieve_all_routes()\n",
    "print(f\"Feasible route candidates: {num_route_candidates}\")\n",
    "\n",
    "# we retrieve the first route in the list\n",
    "# this is equivalent to: route = list_routes[0]\n",
    "route = candidate_holder.retrieve_first_route()\n",
    "\n",
    "# option 2: retrieve the best route by orientation metric\n",
    "# route = candidate_holder.retrieve_best_route_by_orientation()\n",
    "\n",
    "# print out coordinates of the vertices of the reference path\n",
Edmond Irani Liu's avatar
Edmond Irani Liu committed
135
    "print(\"\\nCoordinates [x, y]:\")\n",
Edmond Irani Liu's avatar
Edmond Irani Liu committed
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
    "print(route.reference_path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Visualizing planning results"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The planned routes can be easily visualized with the `draw_route` function. The arguements `draw_route_lanelets` and `draw_reference_path` indicates whether the lanelets of the route and the reference path should be drawn, respectively. The lanelets of the route is colored in cyan."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# retrieve plot limits for better visualization. \n",
    "# option 1: plot limits from reference path\n",
    "# plot_limits = get_plot_limits_from_reference_path(route)\n",
    "# option 2: plot limits from lanelets in the route\n",
    "plot_limits = get_plot_limits_from_routes(route)\n",
    "\n",
    "# determine the figure size for better visualization\n",
    "size_x = 6\n",
    "ratio_x_y = (plot_limits[1] - plot_limits[0]) / (plot_limits[3] - plot_limits[2])\n",
    "fig = plt.figure(figsize=(size_x, size_x / ratio_x_y))\n",
    "fig.gca().axis('equal')\n",
    "\n",
    "draw_route(route, draw_route_lanelets=True, draw_reference_path=False, plot_limits=plot_limits)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now plot the generated reference path as well, which is colored in magenta."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# retrieve plot limits for better visualization. \n",
    "# option 1: plot limits from reference path\n",
    "plot_limits = get_plot_limits_from_reference_path(route)\n",
    "# option 2: plot limits from lanelets in the route\n",
    "# plot_limits = get_plot_limits_from_routes(route)\n",
    "\n",
    "# determine the figure size for better visualization\n",
    "size_x = 6\n",
    "ratio_x_y = (plot_limits[1] - plot_limits[0]) / (plot_limits[3] - plot_limits[2])\n",
    "fig = plt.figure(figsize=(size_x, size_x / ratio_x_y))\n",
    "fig.gca().axis('equal')\n",
    "\n",
    "draw_route(route, draw_route_lanelets=True, draw_reference_path=True, plot_limits=plot_limits)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}