-
Notifications
You must be signed in to change notification settings - Fork 54
/
main_min_example.py
107 lines (84 loc) · 5.01 KB
/
main_min_example.py
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
import sys
import os
# -- Limit number of OPENBLAS library threads --
# On linux based operation systems, we observed a occupation of all cores by the underlying openblas library. Often,
# this slowed down other processes, as well as the planner itself. Therefore, it is recommended to set the number of
# threads to one. Note: this import must happen before the import of any openblas based package (e.g. numpy)
os.environ['OPENBLAS_NUM_THREADS'] = str(1)
import numpy as np
import json
import time
import configparser
import graph_ltpl
"""
This is the main script to run a minimal example of the graph-based local trajectory planner. The minimal example
generates an offline graph for a race track and drives around the track without the interface to an object list.
Furthermore, logs and advanced vehicle dynamics are not considered.
:Authors:
* Tim Stahl <tim.stahl@tum.de>
:Created on:
06.05.2020
"""
# ----------------------------------------------------------------------------------------------------------------------
# IMPORT (should not change) -------------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------------------------------
# top level path (module directory)
toppath = os.path.dirname(os.path.realpath(__file__))
sys.path.append(toppath)
track_param = configparser.ConfigParser()
if not track_param.read(toppath + "/params/driving_task.ini"):
raise ValueError('Specified online parameter config file does not exist or is empty!')
track_specifier = json.loads(track_param.get('DRIVING_TASK', 'track'))
# define all relevant paths
path_dict = {'globtraj_input_path': toppath + "/inputs/traj_ltpl_cl/traj_ltpl_cl_" + track_specifier + ".csv",
'graph_store_path': toppath + "/inputs/stored_graph.pckl",
'ltpl_offline_param_path': toppath + "/params/ltpl_config_offline.ini",
'ltpl_online_param_path': toppath + "/params/ltpl_config_online.ini"
}
# ----------------------------------------------------------------------------------------------------------------------
# INITIALIZATION AND OFFLINE PART --------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------------------------------
# intialize graph_ltpl-class
ltpl_obj = graph_ltpl.Graph_LTPL.Graph_LTPL(path_dict=path_dict,
visual_mode=True,
log_to_file=False)
# calculate offline graph
ltpl_obj.graph_init()
# set start pose based on first point in provided reference-line
refline = graph_ltpl.imp_global_traj.src.\
import_globtraj_csv.import_globtraj_csv(import_path=path_dict['globtraj_input_path'])[0]
pos_est = refline[0, :]
heading_est = np.arctan2(np.diff(refline[0:2, 1]), np.diff(refline[0:2, 0])) - np.pi / 2
vel_est = 0.0
# set start pos
ltpl_obj.set_startpos(pos_est=pos_est,
heading_est=heading_est)
# ----------------------------------------------------------------------------------------------------------------------
# ONLINE LOOP ----------------------------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------------------------------
traj_set = {'straight': None}
tic = time.time()
while True:
# -- SELECT ONE OF THE PROVIDED TRAJECTORIES -----------------------------------------------------------------------
# (here: brute-force, replace by sophisticated behavior planner)
for sel_action in ["right", "left", "straight", "follow"]: # try to force 'right', else try next in list
if sel_action in traj_set.keys():
break
# -- CALCULATE PATHS FOR NEXT TIMESTAMP ----------------------------------------------------------------------------
ltpl_obj.calc_paths(prev_action_id=sel_action,
object_list=[])
# -- GET POSITION AND VELOCITY ESTIMATE OF EGO-VEHICLE -------------------------------------------------------------
# (here: simulation dummy, replace with actual sensor readings)
if traj_set[sel_action] is not None:
pos_est, vel_est = graph_ltpl.testing_tools.src.vdc_dummy.\
vdc_dummy(pos_est=pos_est,
last_s_course=(traj_set[sel_action][0][:, 0]),
last_path=(traj_set[sel_action][0][:, 1:3]),
last_vel_course=(traj_set[sel_action][0][:, 5]),
iter_time=time.time() - tic)
tic = time.time()
# -- CALCULATE VELOCITY PROFILE AND RETRIEVE TRAJECTORIES ----------------------------------------------------------
traj_set = ltpl_obj.calc_vel_profile(pos_est=pos_est,
vel_est=vel_est)[0]
# -- LIVE PLOT (if activated) --------------------------------------------------------------------------------------
ltpl_obj.visual()